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1.  Introduction 


This  report  describes  2  C++  classes:  a  Vector  class  for  performing  vector  algebra  in 
3-dimensional  space  (3D)  and  a  Rotation  class  for  performing  rotations  of  vectors 
in  3D.  These  classes  give  the  programmer  the  ability  to  use  vectors  and  rotation 
operators  in  3D  as  if  they  were  native  types  in  the  C++  language.  Thus,  the  code 
Vector  c  =  a  +  b;  //  addition  of  two  vectors 

performs  vector  addition,  accounting  for  both  magnitude  and  direction  of  the  vectors 
to  satisfy  the  parallelogram  law  of  vector  addition  in  exactly  the  same  way  as 
the  vector  algebra  expression  c  =  a  +  b.  We  also  take  advantage  of  the  operator 
overloading  capabilities  of  C++  so  that  operations  can  be  written  in  a  more  natural 
style,  similar  to  that  of  vector  algebra.  Thus,  the  code 

Vector  c  =  a  *  b;  //  dot  product  of  two  vectors 
expresses  the  scalar  dot  product  c  =  a  •  b. 

Vector  c  =  a  A  b;  //  cross  product  of  two  vectors 
expresses  the  vector  cross  product  c  =  a  x  b,  and 
Vector  c  =  R  *  a;  //  rotation  of  a  vector 

expresses  a  rotation  of  the  vector  a  by  the  rotation  operator  R  to  give  a  vector  c.  A 
reference  sheet  for  each  class  is  made  available  in  Appendix  A  and  Appendix  B. 

Rotations  only  require  an  axis  and  an  angle  of  rotation — which  is  how  they  are 
stored — and  may  be  specified  in  a  number  of  convenient  ways.  We  also  provide 
methods  for  converting  from  the  internal  representation  to  the  equivalent  quaternion 
and  rotation  matrix  representation.  Quaternion  algebra  is  summarized  in  Appendix 
C,  which  then  provides  a  coordinate-free  formula  for  the  rotation  of  a  vector. 

It  is  also  useful  to  describe  rotations  as  a  sequence  of  3  standard  rotations  (Euler 
angles  or  yaw,  pitch,  and  roll),  and  Appendix  D  shows  that  a  rotation  sequence  about 
body  axes  is  equivalent  to  the  same  rotation  sequence  applied  in  reverse  order  about 
fixed  axes.  There  are  a  total  of  12  rotation  sequences  that  can  be  used  to  describe 
the  orientation  of  a  vector.  In  Appendix  E  we  provide  formulas1  for  factoring  an 
arbitrary  rotation  into  each  of  these  rotation  sequences. 

Rotations  are  commonly  described  with  rotation  matrices.  Appendix  F  provides 
formulas  and  source  code  for  converting  between  our  descriptions  of  rotations,  the 
quaternion  representation,  and  the  rotation  matrix. 
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Quaternions  are  also  very  convenient  and  efficient  for  describing  smooth  rotations 
between  2  different  orientations.  Appendix  G  provides  a  derivation  of  the  spherical 
linear  interpolation  (Slerp)  formula  for  this  purpose.  We  also  provide  a  formula  and 
coding  for  fast  incremental  Slerp. 

Sometimes  we  need  to  relate  2  different  orientations  and  find  the  rotation  that  will 
transform  from  one  to  the  other.  This  is  called  the  absolute  orientation  problem  and 
Appendix  H  provides  an  exact  solution  to  this  problem. 

The  Rotation  and  Vector  classes  provide  C++  support  for  all  these  operations.  No 
libraries  are  required  and  there  is  nothing  to  build;  one  merely  needs  to  include  the 
header  file  to  make  use  of  the  class.  (The  Rotation  class  includes  the  Vector  class,  so 
one  only  needs  to  include  Rotation .  h  to  also  make  use  of  the  Vector  class.) 

We  also  provide  examples  of  how  these  classes  can  be  used  to  solve  real  problems. 
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2.  Vector  Class 


The  source  code  for  the  Vector  class  is  completely  self-contained  in  the  header  file 
Vecto  r .  h,  which  is  listed  and  described  in  Appendix  A.  The  program  in  Listing  1 
provides  some  examples  of  how  one  might  use  the  Vector  class. 

Listing  1.  vtest.cpp 


1  //  vtest.cpp:  simple  program  to  demonstrate  basic  usage  of  Vector  class 

2 

3  #include  "Vector. h"  //  only  need  to  include  this  header  file 

4  #include  <iostream> 

5  #include  <cstdlib> 

6  using  namespace  va;  //  vector  algebra  namespace 

7 

8  int  main(  void  )  { 

9 

10  //  let  u  be  a  unit  vector  that  has  equal  components  along  all  3  axes 

11  Vector  u  =  normalize(  Vector(  1.,  1.,  1.  )  ); 

12 

13  //  output  the  vector 

14  std::cout  «  "u  =  "  «  u  «  std::endl; 

15 

16  //  show  that  the  magnitude  is  1 

17  std::cout  «  "magnitude  =  "  «  u.r()  «  std::endl; 

18 

19  //  output  the  polar  angle  in  degrees 

20  std::cout  «  "polar  angle  (deg)  =  "  «  deg(  u. theta ()  )  «  std::endl; 

21 

22  //  output  the  azimuthal  angle  in  degrees 

23  std::cout  «  "azimuthal  angle  (deg)  =  "  «  deg(  u.phi()  )  «  std::endl; 

24 

25  //  output  the  direction  cosines 

26  std::cout  «  "direction  cosines  =  "  «  u.dircos(  X  )  «  "  "  «  u.dircos(  Y  )  «  "  "  «  u.dircos(  Z  )  «  std::endl; 

27 

28  //  let  ihat,  jhat,  khat  be  unit  vectors  along  x-axis,  y-axis,  and  z-axis,  respectively 


29 

Vector  ihat(  1.,  0.,  0.  ),  jhat(  0.,  1 

. ,  0.  ),  khat(  0. ,  0. 

l.  ); 

30 

31 

//  output  the  vectors 

32 

std::cout  «  "ihat  =  "  «  ihat  «  std: 

: endl ; 

33 

std::cout  «  "jhat  =  "  «  jhat  «  std: 

: endl ; 

34 

std::cout  «  "khat  =  "  «  khat  «  std: 

: endl ; 

35 

36 

//  rotate  ihat,  jhat,  khat  about  u  by 

120  degrees 

37 

ihat. rotate (  u,  rad(  120.  )  ); 

38 

jhat. rotate (  u,  rad(  120.  )  ); 

39 

khat. rotate (  u,  rad(  120.  )  ); 

40 

41 

//  output  the  rotated  vectors 

42  std::cout  «  "after  120  deg  rotation  about  u:"  «  std::endl; 

43  std::cout  «  "ihat  is  now  =  "  «  ihat  «  std::endl; 

44  std::cout  «  "jhat  is  now  =  "  «  jhat  «  std::endl; 

45  std::cout  «  "khat  is  now  =  "  «  khat  «  std::endl; 

46 

47  //  define  two  vectors,  a  and  b 

48  Vector  a(  2.,  1.,  -1.  ),  b(  3.,  -4.,  1.  ); 

49  std::cout  «  "a  =  "  «  a  «  std::endl; 

50  std::cout  «  "b  =  "  «  b  «  std::endl; 

51  std::cout  «  "a  +  b  =  "  «  a  +  b  «  std::endl; 

52  std::cout  «  "a  -  b  =  "  «  a  -  b  «  std::endl; 

53 

54  //  compute  and  output  the  dot  product 

55  double  s  =  a  *  b; 

56  std::cout  «  "dot  product,  a  *  b  =  "  «  s  «  std::endl; 

57 

58  //  compute  and  output  the  cross  product 

59  Vector  c  =  a  *  b; 

60  std::cout  «  "cross  product,  a  ~  b  =  "  «  c  «  std::endl; 

61 

62  //  output  the  angle  (deg)  between  a  and  b 

63  std::cout  «  "angle  between  a  and  b  (deg)  =  "  «  deg(  angle (  a,  b  )  )  «  std::endl; 

64 

65  //  compute  and  output  the  projection  of  a  along  b 

66  std::cout  «  "proj(  a,  b  )  =  "  «  proj (  a,  b  )  «  std::endl; 

67 

68  //  rotate  a  and  b  120  deg  about  u 

69  a. rotate (  u,  rad(  120.  )  ); 

70  b. rotate (  u,  rad(  120.  )  ); 

71  std::cout  «  "after  rotating  a  and  b  120  deg  about  u:  "  «  a  «  std::endl; 
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std::cout  «  "a  is  now  =  "  «  a  «  std::endl; 
std::cout  «  "b  is  now  =  "  «  b  «  std::endl; 

//  output  the  angle  (deg)  between  a  and  b 

std::cout  «  "angle  between  a  and  b  (deg)  is  now  =  11  «  deg(  angle (  a,  b  )  )  «  std::endl; 

//  compute  and  output  the  dot  product 
s  =  a  *  b; 

std::cout  «  "dot  product,  a  *  b  is  now  =  "  «  s  «  std::endl; 

//  compute  and  output  the  cross  product 
c  =  a  ~  b; 

std::cout  «  "cross  product,  a  A  b  is  now  =  "  «  c  «  std::endl; 

//  set  a  and  b  to  their  original  values  and  compute  the  cross  product 
a  =  Vector (  2. ,  1. ,  -1.  ) ; 
b  =  Vector (  3. ,  -4. ,  1.  ) ; 
c  =  a  ~  b; 

std::cout  «  "original  cross  product,  c  =  "  «  c  «  std::endl; 

//  rotate  c  120  deg  about  u  and  output 
c. rotate (  u,  rad(  120.  )  ); 

std::cout  «  "now  rotate  c  120  deg  about  u:"  «  std::endl; 
std::cout  «  "c  is  now  =  "  «  c  «  std::endl; 

return  EXIT.SUCCESS; 

} 


Save  this  to  a  file  vest .  cpp  and  compile  it  with  the  command 
g++  -02  -Wall  -o  vtest  vtest.cpp  -Im 
Running  it 


./vtest 

will  print  the  following: 


u  =  0.57735  0.57735  0.57735 
magnitude  =  1 

polar  angle  (deg)  =  54.7356 

azimuthal  angle  (deg)  =  45 

direction  cosines  =  0.57735  0.57735  0.57735 

ihat  =  10  0 

jhat  =010 

khat  =  001 

after  120  deg  rotation  about  u: 

ihat  is  now  =010 

jhat  is  now  =001 

khat  is  now  =10  0 

a  =  2  1  -1 

b  =  3  -4  1 

a  +  b  =  5  -3  0 

a  -  b  =  -1  5  -2 

dot  product,  a  *  b  =  1 

cross  product,  a  /v  b  =  -3  -5  -11 

angle  between  a  and  b  (deg)  =  85.4078 

proj (  a,  b  )  =  0.115385  -0.153846  0.0384615 

after  rotating  a  and  b  120  deg  about  u:  -121 

a  is  now  =-121 

b  is  now  =13-4 

angle  between  a  and  b  (deg)  is  now  =  85.4078 
dot  product,  a  *  b  is  now  =  1 
cross  product,  a  A  b  is  now  =  -11  -3  -5 
original  cross  product,  c  =  -3  -5  -11 
now  rotate  c  120  deg  about  u: 
c  is  now  =  -11  -3  -5 
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3.  Rotation  Class 


Similarly,  the  source  code  for  the  Rotation  class  is  completely  self-contained  in  the 
header  file  Rotation .  h,  which  is  listed  and  described  in  Appendix  B.  The  program 
in  Listing  2  provides  some  basic  examples  of  usage. 


Listing  2.  rtest.cpp 

1  //  rtest . cpp 

2 

3  #include  "Rotation. h" 

4  #include  <iostream> 

5  #include  <cstdlib> 

6  #include  <cmath> 

7  #include  <iomanip> 

8  using  namespace  va; 

9 

10  int  main(  void  )  { 

11 

12  //  declare  two  unit  vectors,  u  and  v 

13  Vector  u  =  Vector!  1.5,  2.1,  3.2  ).unit(),  v  =  Vector!  1.2,  3.5,  -2.3  ).unit(); 

14  std::cout  «  "u  =  "  «  u  «  std::endl; 

15  std::cout  «  "v  =  "  «  v  «  std::endl; 

16 

17  //  let  R  be  the  rotation  specified  by  the  vector  cross  product  u  ^  v 

18  Rotation  R(  u,  v  ); 

19 

20  //  show  that  R  *  u  equals  v 

21  std::cout  «  "R  *  u  =  "  «  R  *  u  «  std::endl; 

22 

23  //  interpolate  a  unit  vector,  w,  halfway  between  u  and  v 

24  Vector  w  =  slerp!  u,  v,  0.5  ); 

25  std::cout  «  "Vector  halfway  between  u  and  v  =  "  «  w  «  std::endl; 

26 

27  //  output  angle  between  u  and  v 

28  std::cout  «  "angle  between  u  and  v  (deg)  =  "  «  deg(  angle!  u,  v  )  )  «  std::endl; 

29 

30  //  output  angle  between  u  and  w 

31  std::cout  «  "angle  between  u  and  w  (deg)  =  "  «  deg(  angle!  u,  w  )  )  «  std::endl; 

32 

33  //  let  Rinv  be  the  inverse  rotation 

34  Rotation  Rinv  =  inverse!  R  ); 

35 

36  //  show  that  Rinv  *  v  equals  u 

37  std::cout  «  "Rinv  *  v  =  "  «  Rinv  *  v  «  std::endl; 

38 

39  Vector  ihat(  1.,  0.,  0.  ),  j hat (  0.,  1.,  0.  ),  khat(  0.,  0.,  1.  ); 

40  R  =  Rotation!  ihat,  jhat,  khat,  jhat,khat,  ihat  ); 

41  std::cout  «  "R  =  "  «  R  «  std::endl; 

42 

43  sequence  s  =  factor!  R,  ZYX  ); 

44  std::cout  «  "yaw  (deg  )  =  "  «  deg(  s. first  )  «  std::endl; 

45  std::cout  «  "pitch  (deg  )  =  "  «  deg(  s. second  )  «  std::endl; 

46  std::cout  «  "roll  (deg  )  =  "  «  deg(  s. third  )  «  std::endl; 

47 

48  R  =  Rotation!  s. first,  s. second,  s. third,  ZYX  ); 

49  std::cout  «  "R  =  "  «  R  «  std::endl; 

50 

51  s  =  factor!  R,  XYZ  ); 

52  std::cout  «  "pitch  (deg  )  =  "  «  deg(  s. first  )  «  std::endl; 

53  std::cout  «  "yaw  (deg  )  =  11  «  deg(  s. second  )  «  std::endl; 

54  std::cout  «  "roll  (deg  )  =  "  «  deg(  s. third  )  «  std::endl; 

55 

56  std : : streamsize  ss  =  std :: cout . precision! ) ; 

57  R  =  Rotation!  s. first,  s. second,  s. third,  XYZ  ); 

58  std:: cout  «  "R  =  "  «  R  «  std::endl; 

59 

60  rng:: Random  rng; 

61  R  =  Rotation!  rng  ); 

62  std:: cout  «  "R  =  "  «  R  «  std::endl; 

63  quaternion  q  =  to_quaternion (  R  ); 

64  std:: cout  «  "the  quaternion  for  this  rotation  is"  «  std::endl; 

65  std:: cout  «  "q  =  "  «  q  «  std::endl; 

66  R  =  Rotation!  q  ) ; 

67  std:: cout  «  "R  =  "  «  R  «  std::endl; 

68  matrix  A  =  to_matrix(  R  ); 

69  std:: cout  «  "the  matrix  for  this  rotation  is"  «  std::endl; 

70  std:: cout  «  std : : setprecision (6)  «  std:: fixed  «  std : : showpos ; 

71  std:: cout  «  A  «  std::endl; 
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std::cout  «  std : : setprecision(ss)  «  std : : noshowpos ; 
R  =  Rotation!  A  ) ; 

std::cout  «  "R  =  "  «  R  «  std::endl; 

u  =  normalize!  Vector!  1. ,  1.,  1.  )  ); 
double  th  =  rad(  120.  ); 

R  =  Rotation!  u,  th  ) ; 
std::cout  «  R  «  std::endl; 

A  =  to_matrix(  A  ); 
std::cout  «  A  «  std::endl; 
q  =  to_quaternion (  R  ); 
std::cout  «  q  «  std::endl; 

return  EXIT_SUCCESS; 

> 


Writing  this  to  a  file  rtest .  cpp,  compiling  and  running  it, 


g++  -02  -Wall  -o  rtest  rtest. cpp  -Im 
./rtest 

produces  the  following  output: 

u  =  0.364878  0.510829  0.778407 

v  =  0.275444  0.803378  -0.527934 

R  *  u  =  0.275444  0.803378  -0.527934 

Vector  halfway  between  u  and  v  =  0.431716  0.886061  0.168873 

angle  between  u  and  v  (deg)  =  84.264 

angle  between  u  and  w  (deg)  =  42.132 

Rinv  *  v  =  0.364878  0.510829  0.778407 

R  =  0.57735  0.57735  0.57735  120 

yaw  (deg  )  =  -90 

pitch  (deg  )  =  -180 

roll  (deg  )  =  -90 

R  =  0.57735  0.57735  0.57735  120 

pitch  (deg  )  =  45 

yaw  (deg  )  =  90 

roll  (deg  )  =  45 

R  =  0.57735  0.57735  0.57735  120 

R  =  0.338694  0.929155  0.148182  174.087 

the  quaternion  for  this  rotation  is 
q  =  0.0515815  0.338243  0.927918  0.147985 

R  =  0.338694  0.929155  0.148182  174.087 

the  matrix  for  this  rotation  is 
-0.765862  +0.612457  +0.195837 

+0.642990  +0.727384  +0.239741 

+0.004383  +0.309530  -0.950880 

R  =  0.338694  0.929155  0.148182  174.086566 
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4.  Example  Applications 


The  preceding  programs  exercised  some  basic  capabilities  of  the  2  classes.  Now  let  us 
consider  some  more  practical  problems  to  see  how  these  classes  aid  in  their  solution. 
Vectors  are  powerful  tools  in  3D  problems  and  it  is  usually  better  to  make  use  of 
the  vectors  directly  in  vector  algebra  rather  than  to  decompose  into  coordinates,  and 
these  classes  allow  us  to  do  that. 

4.1  Impact  Location  on  a  Target  Plate 

Here  is  an  example  problem.  We  have  a  target  plate  that  is  initially  placed  in  the  x-y 
plane,  as  shown  in  Fig.  1,  where  L  is  the  length,  W  the  width,  and  T  the  thickness 
of  the  plate,  defined  such  that  L  >  W  >  T.  We  first  define  a  standard  orientation  of 
the  target  plate  as  the  initial  orientation  and  then  describe  the  operational  procedure 
to  give  it  a  specific,  final  orientation.  This  standard  orientation  is  with  the  center 
at  the  origin  of  a  right-handed  cartesian  ( x ,  y,  z)  coordinate  system  and  its  length 
along  the  x-axis,  its  width  along  the  y- axis,  and  its  thickness  along  the  /--axis,  as 
illustrated  in  Fig.  1 .  Once  it  has  been  given  a  final  orientation,  we  translate  its  center 
to  the  location  rc.  Then  we  shoot  a  fragment  at  the  plate  along  a  ray  from  the  origin 
and  we  want  to  find  the  impact  point  on  the  target,  along  with  its  distance  and  its 
impact  obliquity. 

y 


Fig.  1.  Initial  orientation  of  the  target  plate  in  a  right-handed  Cartesian  coordinate  system 


We  also  define  the  3  unit  vectors  i,  j,  and  k  along  the  x-axis,  y-axis,  and  --axis, 
respectively.  The  target  plate  (entrance)  normal  h  is  initially  aligned  with  k.  The 
final  orientation  is  specified  by  performing  a  pitch-yaw-roll  rotation  sequence,  where 
(f)p  is  pitch  about  the  x-axis,  oy  is  yaw  about  the  y-axis,  and  or  is  roll  about  the 
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z- axis,  as  shown  in  Fig.  2. 


y 


Fig.  2.  Description  of  pitch,  yaw,  and  roll 

Here  we  adopt  the  FATEPEN  (Fast  Air  Target  Encounter  Penetration)2,3  convention 
that  first  pitch  is  applied,  then  yaw,  then  roll.’1'  We  denote  a  rotation  about  the  unit 
axial  vector  e  through  an  angle  0  with  the  notation  /rf,(©).  A  rotation  sequence  can 
thus  be  written  as 

R  =  i?k//(0r)^j'(03,)^i(0p),  (1) 

where  the  order  is  from  right  to  left:  first  pitch  is  applied,  then  yaw,  then  roll.  Notice 
that  we  have  primes  on  the  yaw  and  roll  rotation  operators  to  indicate  that  the  unit 
vectors  get  transformed  after  pitch  is  applied  and  after  yaw  is  applied,  since  the 
rotations  are  applied  to  the  body  axes  of  the  plate.  Now  it  is  a  fundamental  theorem 
of  rotation  sequences  that  rotations  about  the  body  axes  is  equivalent  to  the  same 
sequence  in  reverse  order  about  the  fixed  axes*,  and  since  it  is  more  efficient  to  rotate 
about  fixed  axes,  we  have 

R  =  (2) 

Hence,  the  normal  vector  is  given  by 

h  =  f?i(0p)f?j(02/)f?C;(0r)  k.  (3) 

Once  the  target  plate  is  oriented,  we  specify  the  location  of  its  center  with  the  vector 
rc.  So  the  final  position  and  orientation  of  the  target  plate  is  specified  by  the  pair  of 
vectors  (rc,  n). 

*  There  are  a  total  of  12  different  conventions  in  the  Rotation  class  that  one  may  choose  from. 
See  Appendix  D  for  a  proof. 
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Let  r0  be  the  origin  of  the  fragment  ray  and  let  u  be  a  unit  vector  along  the  ray.  We 
specify  the  orientation  of  u  by  specifying  the  polar  angle  6  and  azimuth  angle  0,  so 
that 

u  =  sin  9  cos  <pi  +  sin  6  sin  0j  +  cos  9 k,  (4) 

and  the  equation  for  the  fragment  ray  is 

r  =  r0  +  tu,  (5) 


where  t  >  0  is  a  scalar  that  measures  the  distance  from  the  origin.  The  target  plane 
(infinite  in  extent)  is  specified  by  all  points  r  such  that 

(r  —  rc)  •  h  =  0,  (6) 

since  the  normal,  by  definition,  is  orthogonal  to  the  plane.  Therefore,  the  intersection 
of  the  fragment  ray  with  the  plane  of  the  target  is  specified  by  the  vector  equation 

(r0  +  tu  -  rc)  •  h  =  0.  (7) 

Solving  for  the  distance  t  gives 

(rr  —  rn)  ■  fi 

t  =  v  c  .  - .  (8) 

u  ■  n 

There  will  be  an  intersection,  provided  u  ■  n  ^  0,  and  the  intersection  point  is 


(rc  -  r0)  •  h  ^ 
r  =  r0  4 - 1 — 1 - u 


u  ■  n 


(9) 


The  impact  obliquity  is  given  by 


6»obi  =  cos  1(|tx  ■  ri|). 


(10) 


The  location  of  the  impact  point  with  respect  to  the  target  center  is 


(rc  -  r 0)  •  n  _ 
r  -  rc  =  r0  —  rc  4 - - — - - u 

u  ■  n 


(11) 


To  determine  if  the  intersection  point  on  the  infinite  target  plane  lies  within  the  finite 
target  plate,  we  apply  the  inverse  rotation  i?-1  to  the  vector  r  —  rc  and  check  to  see 
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if  the  resulting  vector  lies  within  the  plate  dimensions.  Thus,  let 


d  =  R  x(r  -  rc)  (12) 


and  then  we  check  to  see  if 


-L/ 2  <dx<  L/2  and  -  W/2  <  dy  <  Wj 2.  (13) 


Listing  3  is  an  implementation  of  these  vector  equations. 


Listing  3.  geometry.cpp 


//  geometry.cpp:  compute  hit  point  on  an  oriented  target  plate  from  a  general  fragment  ray 

#include  "Rotation. h" 

#include  <iostream> 

#include  <cstdlib> 

#include  <cmath> 

#include  <iomanip> 

#include  <cassert> 
using  namespace  std; 

int  main(  void  )  { 

//  target  dimensions  (in) 

const  double  L  =  96. ,  W  =  48. ,  L_2  =  L  /  2. ,  W_2  =  W  /  2. ; 

//  constant  unit  vectors  for  the  laboratory  frame 

const  va::Vector  I(  1.,  0.,  0  ),  J(  0.,  1.,  0.  ),  K(  0.,  0.,  1.  ); 

std::cout  «  std : : setprecision (3)  «  std:: fixed; 

va::Vector  n  =  K;  //  normal  vector  is  initially  along  K 

va::Vector  u;  //  unit  vector  along  fragment  ray 

va::Vector  rc(  0.,  0.,  -24.  );  //  location  of  the  target  center 

va:: Vector  r0(  0.,  0.,  0.  );  //  origin  of  the  fragment  ray 

//  specify  pitch,  yaw  and  roll  of  the  target  (degrees  converted  to  radians) 


double 

pitch  =  va: 

:  rad  ( 

+30.  ) 

// 

pitch  (converted  to  rad) 

double 

yaw  =  va : 

:  rad  ( 

-35.  ) 

// 

yaw  (converted  to  rad) 

double 

roll  =  va: 

:  rad  ( 

+45.  ) 

// 

roll  (converted  to  rad) 

//  specify  the  target  orientation  with  R  and  inverse  with  Rinv 
va:: Rotation  R(  pitch,  yaw,  roll,  va::XYZ  ); 
va:: Rotation  Rinv  =  va: : inverse (  R  ); 


//  apply  the  orientation  to  the  target  normal 
n  =  R  *  n; 

double  az  =  -35.,  el  =  15.;  //  specify  azimuth  and  elevation  of  fragment  ray  (deg) 

assert (  -360.  <=  az  &&  az  <=  360.  ); 
assert (  0.  <=  el  &&  el  <=  90.  ); 

double  th  =  va::rad(  180.  -  el  ) ;  //  convert  to  polar  angle  (rad) 

double  ph  =  va::rad(  az  ) ;  //  convert  to  azimuthal  angle  (rad) 

//  specify  direction  of  u  vector 

u  =  sin(  th  )  *  cos(  ph  )  *  I  +  sin(  th  )  *  sin(  ph  )  *  J  +  cos(  th  )  *  K; 

double  obi  =  angle(  u,  -n  );  //  obliquity  angle  (rad) 

if  (  obi  >=  M_PI_2  )  { 

cout  «  "ray  missed  target  since  obi  (deg)  =  "  «  obi  *  va::R2D  «  endl; 
exit(  EXIT_SUCCESS  ); 

> 

//  compute  distance  to  (infinite)  target  plane 

double  t=(rc-r0)*n/(u*n); 

va:: Vector  r  =  r0  +  t  *  u;  //  hit  point  in  lab  frame 

va::Vector  d  =  r  -  rc;  //  hit  point  relative  to  the  target  center 

d  =  Rinv  *  d;  //  inverse  rotation  back  to  the  laboratory  reference  frame 

double  xhit  =  d.x(),  yhit  =  d.y(); 

//  check  if  hit  point  on  plane  lies  with  target  plate 

if  (  (  -L_2  <=  xhit  &&  xhit  <=  L_2  )  &&  (  -W_2  <=  yhit  &&  yhit  <=  W_2  )  )  { 

Approved  for  public  release;  distribution  is  unlimited. 


10 


63 

64 

65 

66 

67 

68 

69 

70 

71 

72 

73 

74 

1 

2 

3 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 

21 

22 

23 

24 

25 

26 


cout  «  "ray  hit  target  at  "  «  d  «  "  with  respect  to  the  target  at  the  origin" 
cout  «  "obi  (deg)  =  "  «  obi  *  va::R2D  «  endl; 

cout  «  "distance  =  "  «  t  «  endl; 

> 

else  { 

cout  «  "ray  missed  target  plate"  «  endl; 
cout  «  "xhit  =  "  «  xhit  «  endl; 

cout  «  "yhit  =  "  «  yhit  «  endl; 


return  EXIT.SUCCESS; 


endl ; 


Compiling  and  running  this  program  gives  the  following  output: 


ray  hit  target  at  2.794  -5.560  0.000  with  respect  to  the  target  at  the  origin 
obi  (deg)  =  41.752 
distance  =  22.822 


4.2  Computing  the  Rotation  between  2  Orientations 

We  may  know  the  unit  vectors  i,  j,  k  in  2  different  reference  frames  and  we  need 
to  find  the  rotation  that  takes  one  to  the  other.  Thus,  let  us  suppose  that  we  have  2 
sets  of  orthonormal  (basis)  vectors,  (i,  j,  k)  and  (!',  j',  k'),  and  we  want  to  find  the 
rotation  R  such  that 


i' —  Ri,  j'  =  R  j,  and  k' =  Rk.  (14) 

This  is  a  relatively  simple  problem  for  unit  vectors.  A  much  more  difficult  problem 
is  to  find  the  rotation  between  2  sets  of  3  vectors  when  the  vectors  are  not  unit 
vectors.  A  closed-form  solution  to  this  problem  is  summarized  in  Appendix  H.  Both 
of  these  cases  have  been  implemented  in  the  Rotation  class.  The  Listing  4  provides  a 
demonstration  of  this. 


Listing  4.  r2.cpp 

//  r2.cpp:  find  the  rotation,  given  two  sets  of  vectors  related  by  a  pure  rotation 

#include  "Rotation. h" 

#include  <iostream> 

#include  <cstdlib> 

#include  <cmath> 

#include  <iomanip> 

#include  <cassert> 
using  namespace  std; 

int  main(  int  argc,  char*  argv[]  )  { 

//  constant  unit  vectors  for  the  laboratory  frame 

const  va::Vector  i(  1.,  0.,  0  ),  j(  0.,  1.,  0.  ),  k(  0.,  0.,  1.  ); 


double  p  =  0.,  y  =  0.,  r  =  0.; 
if  (  argc  ==  4  )  { 


p  =  atof(  argv[l]  ) 

y  =  atof(  argv[2]  ) 

r  =  atof(  argv[3]  ) 

> 


std:: cout  «  std : : setprecision (3)  «  std:: fixed; 

//  specify  pitch,  yaw  and  roll  of  the  target  (degrees  converted  to  radians) 
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27  double  pitch  =  va::rad(  p  ); 

28  double  yaw  =  va : : rad (  y  ) ; 

29  double  roll  =  va::rad(  r  ); 

30 

31  //  specify  the  rotation 

32  va:: Rotation  R(  pitch,  yaw,  roll,  va::XYZ  ); 

33 

34  //  apply  the  rotation  to  the  basis  vectors 

35  va::Vector  ip  =  R  *  i; 

36  va:: Vector  jp  =  R  *  j; 

37  va:: Vector  kp  =  R  *  k; 

38 

39  cout  «  "i  =  "  «  i  «  endl; 

40  cout  «  "j  =  "  «  j  «  endl; 

41  cout  «  "k  =  "  «  k  «  endl; 

42 

43  cout  «  "ip  =  "  «  ip  «  endl; 

44  cout  «  "jp  =  "  «  jp  «  endl; 

45  cout  «  "kp  =  "  «  kp  «  endl; 

46 

47  cout  «  endl  «  "Now  we  find  the  rotation"  «  endl; 

48 

49  va:: Rotation  Rl(  i,  j,  k,  ip,  jp,  kp  ); 

50 

51  cout  «  "Applying  the  found  rotation  to  i,  j,  k  gives"  «  endl; 

52 

53  ip  =  R1  *  i; 

54  j  p  =  R1  *  j ; 

55  kp  =  R1  *  k; 

56 

57  cout  «  "ip  =  "  «  ip  «  endl; 

58  cout  «  "jp  =  "  «  jp  «  endl; 

59  cout  «  "kp  =  "  «  kp  «  endl; 

60 

61  cout  «  endl  «  "Now  suppose  we  want  to  factor  this  rotation  into  a  FATEPEN  sequence"  «  endl; 

62 

63  va:: sequence  s  =  va:: factor!  Rl,  va::XYZ  ); 

64 

65  cout  «  "This  gives:"  «  endl; 

66  cout  «  va::deg(  s. first  )  «  endl; 

67  cout  «  va::deg(  s. second  )  «  endl; 

68  cout  «  va::deg(  s. third  )  «  endl; 

69 

70  cout  «  endl  «  "Now  for  something  completely  different:  Start  with  the  non-unit  vectors"  «  endl; 

71 

72  va:: Vector  a(  1.,  2.,  3.  ),  b(  -1.,  2.,  4.  ),  c(  4.,  3.,  9.  ); 

73  va:: Vector  ap,  bp,  cp; 

74 

75  cout  «  "a  =  "  «  a  «  endl; 

76  cout  «  "b  =  "  «  b  «  endl; 

77  cout  «  "c  =  "  «  c  «  endl; 

78 

79  ap  =  R  *  a; 

80  bp  =  R  *  b; 

81  cp  =  R  *  c; 

82 

83  cout  «  "ap  =  "  «  ap  «  endl; 

84  cout  «  "bp  =  "  «  bp  «  endl; 

85  cout  «  "cp  =  "  «  cp  «  endl; 

86 

87  cout  «  endl  «  "Now  we  find  the  rotation  that  takes  (a,b,c)  to  (ap,bp,cp)"  «  endl; 

88 

89  va:: Rotation  R2(  a,  b,  c,  ap,  bp,  cp  ); 

90 

91  cout  «  "Applying  the  found  rotation  to  a,  b,  c  gives"  «  endl; 

92 

93  ap  =  R2  *  a; 

94  bp  =  R2  *  b; 

95  cp  =  R2  *  c; 

96 

97  cout  «  "ap  =  "  «  ap  «  endl; 

98  cout  «  "bp  =  "  «  bp  «  endl; 

99  cout  «  "cp  =  "  «  cp  «  endl; 

100 

101  cout  «  endl  «  "Also,  suppose  we  want  to  factor  this  rotation  into  a  FATEPEN  sequence"  «  endl; 

102 

103  s  =  va:: factor!  R2,  va::XYZ  ); 

104 

105  cout  «  "This  gives:"  «  endl; 

106  cout  «  va::deg(  s. first  )  «  endl; 

107  cout  «  va::deg(  s. second  )  «  endl; 

108  cout  «  va::deg(  s. third  )  «  endl; 

109 

110  return  EXIT-SUCCESS; 

HI  } 
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We  don’t  have  to  worry  about  whether  the  vectors  are  unit  vectors  or  not,  the  Rotation 
class  figures  that  out  and  performs  the  simpler  method  when  it’s  able.  Compiling 
and  running  this  program  with  the  command 
. /r2  60.  -45.  15. 


gives  the  following  output: 


i  =  1.000  0.000  0.000 
j  =  0.000  1.000  0.000 
k  =  0.000  0.000  1.000 
ip  =  0.683  -0.462  0.566 
jp  =  -0.183  0.641  0.745 
kp  =  -0.707  -0.612  0.354 

Now  we  find  the  rotation 

Applying  the  found  rotation  to  i,  j,  k  gives 
ip  =  0.683  -0.462  0.566 
jp  =  -0.183  0.641  0.745 
kp  =  -0.707  -0.612  0.354 

Now  suppose  we  want  to  factor  this  rotation  into  a  FATEPEN  sequence 
This  gives: 

60.000 

-45.000 

15.000 

Now  for  something  completely  different:  Start  with  the  non -unit  vectors 

a  =  1.000  2.000  3.000 

b  =  -1.000  2.000  4.000 

C  =  4.000  3.000  9.000 

ap  =  -1.804  -1.016  3.116 

bp  =  -3.877  -0.704  2.339 

cp  =  -4.181  -5.435  7.680 

Now  we  find  the  rotation  that  takes  (a,b,c)  to  (ap,bp,cp) 

Applying  the  found  rotation  to  a,  b,  c  gives 
ap  =  -1.804  -1.016  3.116 
bp  =  -3.877  -0.704  2.339 
cp  =  -4.181  -5.435  7.680 

Also,  suppose  we  want  to  factor  this  rotation  into  a  FATEPEN  sequence 
This  gives: 

60.000 

-45.000 

15.000 
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4.3  Deflected  Spall  Cone  from  an  Oblique  Shot 


When  an  overmatching  penetrator  perforates  target  armor,  it  generates  a  cone  of  spall 
fragments  from  the  back  of  the  armor.  To  better  understand  the  threat  this  poses, 
individual  spall  fragments  are  registered  as  holes  in  a  series  of  witness  plates  that 
are  located  some  distance  behind  the  target,  typically  24  inches.  In  the  case  of  an 
oblique  shot ,  the  target  is  at  an  obliquity  angle  a  with  respect  to  the  shotline,  and  the 
witness  plates  are  typically  at  half  that  angle,  or  a /2,  as  depicted  in  Fig.  3.  Although 
there  are  usually  5  plates  in  the  witness  pack  during  test  conditions,  here  we  will 
only  consider  the  first  plate  of  the  pack. 


What  we  are  going  to  do  here  is  first  make  use  of  vector  algebra  to  describe  the 
coordinate  system  and  the  geometry  of  the  test  layout,  along  with  the  operational 
procedure  to  orient  the  target  and  witness  plate.  Following  that,  we  will  then  show 
that  it  is  easy  to  implement  this  into  a  C++  program  by  making  use  of  the  Vector  and 
Rotation  classes  and  by  a  straightforward  translation  of  the  vector  equations. 


Approved  for  public  release;  distribution  is  unlimited. 
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Of  course  we  need  a  coordinate  system,  but  the  coordinate  system  is  constructed 
from  the  test  geometry,  not  vice  versa.  Once  the  coordinate  system  is  established,  we 
can  pretty  much  ignore  the  actual  coordinates  and  just  deal  with  vector  quantities. 

So  we  begin  with  the  direction  of  the  shot  line  and  take  this  to  be  along  the  negative 
z-axis.  Then  we  place  the  target  plate  in  standard  orientation,  with  its  center  at  the 
origin,  perpendicular  to  the  shotline,  its  length  along  the  at-axis,  and  its  width  along 
the  y- axis.  The  same  applies  to  the  witness  plate.  These  are  just  initial  orientations; 
we  will  subsequently  describe  the  procedure  to  put  them  in  their  final  orientations  and 
positions.  Let  us i  be  a  unit  vector  along  the  initial  shotline.  In  our  case,  wsi  =  — k, 
and  remains  fixed. 

Next  we  orient  the  target  with  respect  to  the  shotline.  There  are  at  least  a  couple 
of  convenient  ways  to  do  this.  We  could  specify  directly  the  final  orientation  of 
the  target,  in  which  case  the  Rotation  operator  would  be  generated  from  the  cross 
product  from  initial  to  final  orientation.*  Another  method  is  to  specify  the  rotation 
sequence  that  will  take  it  from  its  initial  orientation  to  its  final  orientation.  We  shall 
use  that  method  here  and  adopt  the  FATEPEN2  3  convention  of  first  pitch  about  the 
at-axis,  followed  by  yaw  about  the  y-axis,  and  end  with  roll  about  the  z-axis,  so  that 

Rm  =  Ri{(f)p)R3((j)y)Rt((j>r),  (15) 

where  the  order  goes  from  right  to  left  and  is  reversed  since  we  rotate  about  fixed 
axes,  not  body  axes.*  Since  the  initial  orientation  is  — k,  the  final  orientation  is 

f^trgt  ffligt  (  k).  (16) 

The  target  obliquity  is  labeled  a,  and 

a  =  #trgt  =  COS_1(|'Usi  •  ntrgt|).  (17) 

Next  we  want  to  orient  the  witness  plate  to  be  at  half  the  obliquity  angle  of  the  target. 

*Given  an  initial  and  a  final  orientation.  fi\  and  n2 ,  respectively,  the  Rotation  operator  that 
performs  this  rotation  is  Rfi{0),  where 

„  fi\  X  fl2  a  —1/1-'  -  |\ 

Ilni  *  n2\\ 

*See  Appendix  D  for  a  justification. 
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This  can  be  achieved  with  the  aid  of  the  spherical  linear  interpolation  function^ 


riwp  =  slerp(— k,  ritrgt,  1/2), 


(18) 


which  gives  us  a  unit  vector  that  is  half  the  angle  between  — k  and  ritrgt,  and  the 
witness  plate  rotation  from  standard  orientation  is 

RwP  =  Rn{a/ 2)  where  h  =  (  x  n^P  (19) 

II  ( — k)  x  nwp|| 

We  might  want  to  factor  this  rotation  into  a  pitch-yaw-roll  rotation  sequence,  since 
that  would  give  us  an  operational  procedure  for  achieving  the  orientation  of  the 
witness  plate  such  that  it  is  precisely  at  one-half  the  target  orientation. 


Now  let  us  consider  the  deflection.  If  risi  and  ritrgt  are  not  in  the  same  direction  (i.e., 
the  target  is  at  non-zero  obliquity)  then  their  vector  cross  product  is  non-zero  and  we 
can  define  the  unit  vector 


,  ft  si  X  ri^gt 

W  =  — - — 

Msi  x  ns i 


(20) 


The  deflected  shot  line  is  obtained  by  rotating  ris[  about  the  unit  vector  w  through 
the  rotation  angle  0,  where  0  <  (3  <  a.  Our  notation  for  this  rotation  is  ri*(/5).  The 
witness  plate  has  its  center  at  the  location  rc  with  respect  to  the  rear  of  the  target 
plate,  typically  24  inches. 


Now  consider  spall  fragments  that  are  coming  off  from  the  rear  of  the  target.  We  can 
parametrize  these  fragments  with  the  polar  angle  9,  measured  from  the  -2-axis,  and 
the  azimuthal  angle  0,  measured  from  the  x-axis. 


rifrag 


sin(7T  —  6)  cos  0i  +  sin(7r  —  6)  sin0j  +  cos(7r  —  9)  k 
sin  6  cos  0i  +  sin  6  sin  0  j  —  cos  9  k 


(21) 


The  spall  fragments  then  get  deflected  according  to  the  equation 

^frag  =  ^*(0)  rifrag.  (22) 


The  (deflected)  fragment  will  travel  in  a  straight  line  until  it  encounters  the  witness 


^See  Appendix  G. 
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plate,  so  the  equation  for  its  trajectory  is 


(23) 


where  the  scalar  t  >  0  measures  distance  from  the  spall  origin.  The  equation  of  the 
plane  of  the  witness  plate  is  the  set  of  all  points  r  that  satisfy 


(r  -  rc)  •  nwp  =  0. 


(24) 


Substituting  Eq.  23  into  Eq.  24  and  solving  for  t  gives  for  the  distance 


^frag  ’  nwp 


Hence,  the  hit  point  on  the  witness  plate  is 


’'hit  = 


(25) 


(26) 


To  get  the  location  of  the  hit  point  with  respect  to  the  witness  plate  center,  we  apply 


^wp(rhit  -  rc),  (27) 

where  R~ *  is  the  inverse  of  the  witness  plate  rotation,  given  by  Eq.  19.  The  obliquity 
angle  of  each  frag  as  it  strikes  the  witness  plate  is 


6Ui  =  cos  1(|t4ag-nwp|)- 


(28) 


The  program  in  Listing  5  is  an  implementation  of  these  equations  and  procedures. 


Listing  5.  oblique.cpp 

//  oblique.cpp:  geometry  for  oblique  shots  using  a  right-handed  cartesian  coordinate  system 
//  shotline  is  fixed  along  the  -z  axis 

//  initial  orientation  of  target  is  in  the  x-y  plane  with  center  at  the  origin 

//  initial  orientation  of  witness  plate  is  also  in  the  x-y  plane,  but  with  center  at  z  =  -24  inches 

#include  "Rotation . h" 

#include  <iostream> 

#include  <cstdlib> 
using  namespace  va; 
using  namespace  std; 


int  main(  int  argc,  char*  argv[]  )  { 


const  int  N  =  300 

//  number  of  frags  on  the  spall  cone 

const  double  WP_TRGT_0BL  =0.5 

//  ratio  of  witness  plate  obliquity  to  target  obliquity  (typically  0.5) 

const  double  CONE_HALF_ANGLE  =  30. 

//  cone  half  angle  (deg) 

Vector  ihat(  1.,  0.,  0.  ),  jhat(  0.,  1.,  0.  ),  khat(  0.,  0.,  1.  );  //  basis  vectors 

Approved  for  public  release;  distribution  is  unlimited. 
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Vector  u_sl  =  -khat;  //  shotline  fixed  along  -z  axis 
Vector  n_trgt  =  -khat;  //  initial  orientation  of  target 
Vector  n_wp  =  -khat;  //  initial  orientation  of  witness  plate 

double  pitch_t  =0.,  yaw_t  =0.,  roll_t  =0.,  def  =  0.5; 
if  (  argc  ==  5  )  { 

pitch_t  =  rad(  atof(  argv[l]  )  );  //  pitch  (converted  to  rad)  about  x-axis 

yaw_t  =  rad(  atof(  argv[2]  )  );  //  yaw  (converted  to  rad)  about  y-axis 

roll_t  =  rad(  atof(  argv[3]  )  );  //  roll  (converted  to  rad)  about  z-axis 

def  =  atof(  argv[4]  );  //  deflection  (dimensionless)  ranges  from  0  to  1 

} 

//  orient  the  target  by  specifying  pitch -yaw- roll  rotation  sequence  (totally  arbitrary) 

Rotation  R_trgt(  pitch_t,  yaw_t,  roll_t,  XYZ  );  //  construct  the  rotation 

n_trgt  =  R_trgt  *  n_trgt;  //  apply  the  rotation  to  the  target 

double  alpha  =  angle(  u_sl,  n_trgt  );  //  compute  obliquity  of  target  (rad) 

//  orient  the  witness  plate  using  spherical  linear  interpolation 
Vector  n  =  slerp(  -khat,  n_trgt,  WP_TRGT_OBL  );  //  final  orientation 

Rotation  R_wp(  ihat,  0.  );  //  initialize  rotation  to  zero 

if  (  alpha  >  0.  &&  WP_TRGT_OBL  >  0.  )  //  case  of  non -zero  obliquity 

R_wp  =  Rotation(  n_wp,  n  );  //  generate  the  rotation  from  the  cross  product 

n_wp  =  n;  //  orient  the  witness  plate 

Rotation  R_wp_inv  =  -R_wp;  //  construct  inverse  rotation  of  witness  plate 

//  factor  witness  plate  rotation  into  a  pitch -yaw- roll  rotation  sequence  and  output 
sequence  s  =  factor(  R_wp,  XYZ  ); 

clog  «  "Witness  Plate  Orientation  (pitch -yaw- roll  sequence):"  «  endl; 

clog  «  "pitch  (deg)  =  "  «  deg(  s. first  )  «  endl; 

clog  «  "yaw  (deg)  =  "  «  deg(  s. second  )  «  endl; 

clog  «  "roll  (deg)  =  "  «  deg(  s. third  )  «  endl; 

//  output  target  and  witness  plate  obliquity 

clog  «  "target  obliquity  (deg)  =  "  «  deg(  alpha  )  «  endl; 

clog  «  "wp  obliquity  (deg)  =  "  «  deg(  angle (  u_sl,  n_wp  )  )  «  endl; 

double  beta  =  def  *  alpha;  //  deflection  angle  (rad) 

clog  «  "spall  cone  deflection  (deg)  =  "  «  deg(  beta  )  «  endl; 

Rotation  R_def(  ihat,  0.  );  //  rotation  for  deflection  is  initially  zero 

Vector  w  =  u_sl  A  n_trgt;  //  w  is  cross  product  of  shotline  and  target  normal 

if  (  w.magO  >  0.  )  {  //  w  is  nonzero  iff  target  at  obliquity 

w  =  normalize(  w  );  //  make  w  into  a  unit  vector 

R_def  =  Rotation (  w,  beta  );  //  rotation  for  deflection 

} 

Vector  rc  =  -24.  *  khat;  //  location  of  center  of  witness  plate 
Vector  u_frag,  r_hit,  r; 

double  th  =  M_PI  -  rad(  CONE_HALF_ANGLE  ),  ph,  t; 

for  (  int  i  =  0;  i  <  N;  i++  )  {  //  generate  frags  on  the  spall  cone 

ph  =  2.  *  M_PI  *  i  /  double (  N  ); 

u_frag  =  sin(  th  )  *  cos(  ph  )  *  ihat  +  sin(  th  )  *  sin(  ph  )  *  jhat  +  cos(  th  )  *  khat; 

iLfrag  =  R_def  *  u_frag; 
t  =  (  rc  *  n_wp  )  /  (  u_frag  *  n_wp  ); 
r_hit  =  t  *  u_frag; 

r  =  R_wp_inv  *  (  r_hit  -  rc  ) ; 

cout  «  r.x()  «  "\t"  «  r.y()  «  endl; 

} 

return  EXIT-SUCCESS; 


Compiling  and  running  this  program  with  the  command 


./oblique  45.  -34.2644  15.  0.5  >  output 

prints  the  following  back  to  the  screen: 


Witness  Plate  Orientation  (pitch -yaw- roll  sequence): 

pitch  (deg)  =  20.246 

yaw  (deg)  =  -18.4381 

roll  (deg)  =  3.31976 

target  obliquity  (deg)  =  54.2403 

wp  obliquity  (deg)  =  27.1201 

spall  cone  deflection  (deg)  =  27.1201 
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The  output  file  contains  the  impact  points  on  the  witness  plate  from  the  for  loop 
(lines  69-79  of  oblique. cpp).  Fig.  4  shows  the  resulting  plots. 


Fig.  4.  Plots  of  the  30°  (half-angle)  spall  cone  on  the  witness  plate  for  a  range  of  deflections.  On 
the  left,  the  deflection  parameter  is  0,  0.25,  0.5,  0.75,  and  1  as  the  ellipses  go  from  lower  left 
to  upper  right.  On  the  right  is  the  plot  for  a  deflection  of  0.5,  which  matches  the  witness  plate 
obliquity  and  thus  results  in  a  circular  spall  cone.  Target  pitch,  yaw,  and  roll  are  fixed  at  45°, 
-34.2644°,  and  15°,  respectively  giving  a  target  obliquity  for  all  theses  cases  of  54.2403°. 


5.  Conclusion 

It  should  be  clear  from  the  examples  that  the  Vector  and  Rotation  classes  provide 
robust  support  for  performing  3D  vector  algebra  in  C++  programs.  To  make  use 
of  the  Vector  class,  simply  include  the  Vecto  r .  h  class  file  and  to  make  use  of  both 
the  Vector  and  Rotation  classes,  simply  include  the  Rotation .  h  class  file  in  the  C++ 
program. 
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Listing  A-l.  Vector.h 


1  //  Vector.h:  Definition  &  implementation  of  class  for  the  algebra  of  3D  vectors 

2  //  R.  Saucier,  February  2000  (Revised  June  2016) 

3 

4  #ifndef  VECT0R_3D_H 

5  #define  VECT0R_3D_H 

6 

7  #include  <cstdlib> 

8  #include  <cassert> 

9  #include  <cmath> 

10  #include  <iostream> 

11 

12  namespace  va  {  //  vector  algeba  namespace 

13 

14  enum  rep  {  CART,  POLAR  };  //  vector  representation  (cartesian  or  polar) 

15  enum  comp  {  X,  Y,  Z  };  //  for  referencing  cartesian  components 

16  const  double  R2D(  180.  /  M_PI  );  //  for  converting  radians  to  degrees 

17  const  double  D2R(  M_PI  /  180.  );  //  for  converting  degrees  to  radians 

18  inline  double  deg(  const  double  rad  )  {  return  rad  *  R2D;  }  //  convert  radians  to  degrees 

19  inline  double  rad(  const  double  deg  )  {  return  deg  *  D2R;  }  //  convert  degrees  to  radians 

20  //  IEEE  754  standard  has  53  significant  bits  or  15  decimal  digits  of  accuracy,  so  anything  smaller  is  not  significant 

21  static  const  long  double  TOL  =  l.e-15; 

22 

23  class  Vector  { 

24 

25  //  friends  list 

26  //  overloaded  arithmetic  operators 

27 

28  friend  Vector  operator+(  const  Vector&  a,  const  Vector&  b  )  {  //  addition 

29 

30  return  Vector(  a._x  +  b.__x,  a._y  +  b._y,  a._z  +  b._z  ); 

31  > 

32 

33  friend  Vector  operator- (  const  Vector&  a,  const  Vector&  b  )  {  //  subtraction 

34 

35  return  Vector(  a._x  -  b._x,  a._y  -  b._y,  a._z  -  b._z  ); 

36  } 

37 

38  friend  Vector  operator*(  const  Vector&  v,  double  s  )  {  //  right  multiply  by  scalar 

39 

40  return  Vector(  s  *  v._x,  s  *  v._y,  s  *  v._z  ); 

41  } 

42 

43  friend  Vector  operator* (  double  s,  const  Vector&  v  )  {  //  left  multiply  by  scalar 

44 

45  return  Vector(  s  *  v._x,  s  *  v._y,  s  *  v._z  ); 

46  > 

47 

48  friend  Vector  operator/(  const  Vector^  v,  double  s  )  {  //  division  by  scalar 

49 

50  assert (  s  !=  0.  ) ; 

51  return  Vector(  v._x  /  s,  v._y  /  s,  v._z  /  s  ); 

52  > 

53 

54  //  overloaded  algebraic  operators 

55 

56  friend  inline  double  operator*(  const  Vector&  a,  const  Vector&  b  )  {  //  dot  product 

57 

58  double  c(  a._x  *  b._x  + 

59  a._y  *  b._y  + 

60  a ._z  *  b._z  ) ; 

61  if  (  fabs(c)  <  TOL  )  c  =  0.0L;  //  set  precision  to  be  no  more  than  15  digits 

62  return  c; 

63  } 

64 

65  friend  inline  Vector  operator^  const  Vector&  a,  const  Vector&  b  )  {  //  cross  product 

66 

67  return  Vector(  a._y  *  b._z  -  a._z  *  b._y, 

68  a._z  *  b._x  -  a._x  *  b._z, 

69  a._x  *  b._y  -  a._y  *  b._x  ) ; 

70  > 

71 

72  //  access  functions 

73 

74  friend  double  x(  const  Vector&  v  )  {  //  x- coordinate 

75 

76  return  v._x; 

77  } 

78 

79  friend  double  y(  const  Vector&  v  )  {  //  y- coordinate 

80 

81  return  v._y; 

82  } 

83 
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84 

friend  double  z(  const  Vectors  v  )  { 

//  z- coordinate 

85 

86 

return  v._z; 

87 

} 

88 

89 

friend  double  r(  const  Vectors  v  )  { 

//  magnitude  of  vector 

90 

91 

return  v._mag ( ) ; 

92 

} 

93 

94 

friend  double  theta (  const  Vectors  v 

)  {  //  polar  angle  (radians) 

95 

96 

return  v. theta () ; 

97 

> 

98 

99 

friend  double  phi (  const  Vectors  v  ) 

{  //  azimuthal  angle  (radians) 

100 

101 

return  v.phi() ; 

102 

> 

103 

104 

friend  double  norm(  const  Vectors  v  ) 

{  //  norm  or  magnitude 

105 

106 

return  v._mag ( ) ; 

107 

} 

108 

109 

friend  double  mag(  const  Vectors  v  ) 

{  //  magnitude 

110 

111 

return  v._mag ( ) ; 

112 

} 

113 

114 

friend  double  scalar(  const  Vectors  v 

)  {  //  magnitude 

115 

116 

return  v._mag ( ) ; 

117 

} 

118 

119 

friend  double  angle (  const  Vectors  a. 

const  Vectors  b  )  {  //  angle  (radians 

)  between  vectors 

120 

121 

double  s  =  a. unit ()  *  b.unit(); 

122 

if  (  s  >=  1.  ) 

123 

return  0. ; 

124 

else  if  (  s  <=  -1.  ) 

125 

return  M_PI; 

126 

else 

127 

return  acos(  s  ); 

128 

} 

129 

130 

friend  Vector  unit(  const  Vectors  v  ) 

{  //  unit  vector  in  same  direction 

131 

132 

return  v.unit() ; 

133 

} 

134 

135 

friend  Vector  normalize(  const  Vectors  v  )  {  //  returns  a  unit  vector  in  : 

same  direction 

136 

137 

return  v.unit() ; 

138 

> 

139 

140 

friend  double  dircos(  const  Vectors  v 

,  const  compS  i  )  {  //  direction  cosine 

141 

142 

return  v.dircos(  i  ) ; 

143 

} 

144 

145 

friend  Vector  proj (  const  Vectors  a, 

//  projection  of  first  vector 

146 

const  Vectors  b  ) 

{  //  along  second  vector 

147 

148 

return  a. proj  (  b  ) ; 

149 

} 

150 

151 

//  overloaded  stream  operators 

152 

153 

friend  std : :  istreamS  operator»(  std: 

: istreamS  is,  Vectors  v  )  {  //  input 

vector 

154 

155 

return  is  »  v._x  »  v._y  »  v._z; 

156 

} 

157 

158 

friend  std : : ostreamS  operator«(  std: 

: ostreamS  os,  const  Vectors  v  )  {  // 

output  vector 

159 

160 

Vector  a(  v  ) ; 

161 

a ._set_precision ( ) ; 

162 

return  os  «  a._x  «  "  "  «  a._^y  «  "  "  «  a._z; 

163 

> 

164 

165 

public : 

166 

167 

Vector(  double  x,  double  y,  double  z, 

//  constructor  (cartesian  or  polar 

168 

rep  mode  =  CART  )  { 

//  with  cartesian  as  default) 
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169 


170 

if  (  mode  ==  CART  )  {  //  cartesian  form 

171 

this->_x  =  x; 

172 

this->_y  =  y; 

173 

this->_z  =  z; 

174 

} 

175 

else  if  (  mode  ==  POLAR  )  //  polar  form 

176 

_setCartesian (  x,  y,  z  ); 

177 

else  { 

178 

std::cerr  «  "Vector:  mode  must  be  either  CART  or  POLAR"  «  std::endl; 

179 

exit (  EXIT_ FAILURE  ); 

180 

} 

181 

} 

182 

183 

Vector(  void  )  :  _x(  0.  ),  _y(  0.  ),  _z(  0.  )  {  //  default  constructor 

184 

} 

185 

186 

-Vector(  void  )  {  //  default  destructor 

187 

} 

188 

189 

Vector(  const  Vectors  v  )  :  _x(  v._x  ),  //  copy  constructor 

190 

-y(  v._y  ), 

191 

-z(  v._z  )  { 

192 

> 

193 

194 

Vectors  operator=(  const  Vectors  v  )  {  //  assignment  operator 

195 

1% 

if  (  this  !=  Sv  )  { 

197 

_x  =  v._x; 

198 

-y  =  v._y; 

199 

N 

> 

N 

200 

} 

201 

return  *this; 

202 

} 

203 

204 

// 

overloaded  arithmetic  operators 

205 

206 

Vectors  operator+=(  const  Vectors  v  )  {  //  addition  assignment 

207 

208 

_x  +=  v._x; 

209 

-y  +=  v._y; 

210 

_z  +=  v._z; 

211 

return  *this; 

212 

> 

213 

214 

Vectors  operator-=(  const  Vectors  v  )  {  //  subtraction  assignment 

215 

216 

_x  -=  v._x; 

217 

_y  -=  v._y; 

218 

_z  -=  v._z; 

219 

return  *this; 

220 

> 

221 

222 

Vectors  operator*=(  double  s  )  {  //  multiplication  assignment 

223 

224 

_x  *=  s; 

225 

_y  *=  s; 

226 

_z  *=  s; 

227 

return  *this; 

228 

} 

229 

230 

Vectors  operator/=(  double  s  )  {  //  division  assignment 

231 

232 

assert (  s  !=  0.  ) ; 

233 

_x  /=  s; 

234 

-y  /=  s; 

235 

-z  /=  s; 

236 

return  *this; 

237 

> 

238 

239 

Vector  operator- (  void  )  {  //  negative  of  a  vector 

240 

241 

return  Vector(  -_x,  -_y,  -_z  ); 

242 

> 

243 

244 

const  doubles  operator!] (  comp  i  )  const  {  //  index  operator  (component) 

245 

246 

if  (  i  ==  X  )  return  _x; 

247 

if  (  i  ==  Y  )  return  _y; 

248 

if  (  i  ==  Z  )  return  _z; 

249 

std::cerr  «  "Vector:  Array  index  out  of  range;  must  be  X,  Y,  or  Z"  «  std::endl; 

250 

exit(  EXIT_ FAILURE  ); 

251 

} 

252 

253 

// 

access  functions 
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254 

255  double  x(  void  )  const  {  //  x- component 

256 

257  return  _x; 

258  } 

259 

260  double  y(  void  )  const  {  //  y-component 

261 

262  return  _y; 

263  > 

264 

265  double  z(  void  )  const  {  //  z- component 

266 

267  return  _z; 

268  > 

269 

270  double  r(  void  )  const  {  //  magnitude 

271 

272  return  _mag(); 

273  > 

274 

275  double  theta(  void  )  const  {  //  polar  angle  (radians) 

276 

277  return  _theta(); 

278  } 

279 

280  double  phi (  void  )  const  {  //  azimuthal  angle  (radians) 

281 

282  return  _phi ( ) ; 

283  } 

284 

285  double  norm(  void  )  const  {  //  norm  or  magnitude 

286 

287  return  _mag ( ) ; 

288  > 

289 

290  double  mag(  void  )  const  {  //  magnitude 

291 

292  return  _mag(); 

293  > 

294 

295  operator  doublet  void  )  const  {  //  conversion  operator  to  return  magnitude 

296 

297  return  _mag(); 

298  } 

299 

300  //  utility  functions 

301 

302  Vector  unit(  void  )  const  {  //  returns  a  unit  vector 

303 

304  double  m  =  _mag(); 

305  if  (  m  >  0.  )  return  Vector(  _x  /  m,  _y  /  m,  _z  /  m  ) ; 

306  std::cerr  «  "Vector:  Cannot  make  a  unit  vector  from  a  null  vector"  «  std::endl; 

307  exit (  EXIT_ FAILURE  ); 

308  > 

309 

310  Vector  normalize (  void  )  const  {  //  synonym  for  unit 

311 

312  return  unit(); 

313  } 

314 

315  double  dircos(  const  compS  i  )  const  {  //  direction  cosine 

316 

317  if  (  i  ==  X  )  return  _x  /  _mag(); 

318  if  (  i  ==  Y  )  return  _y  /  _mag(); 

319  if  (  i  ==  Z  )  return  _z  /  _mag(); 

320  std::cerr  «  "Vector  dircos:  comp  out  of  range;  must  be  X,  Y,  or  Z"  «  std::endl; 

321  exit(  EXIT_ FAILURE  ); 

322  } 

323 

324  Vector  proj (  const  Vector&  e  )  const  {  //  projects  onto  the  given  vector 

325 

326  Vector  u  =  e.unit(); 

327  return  u  *  (  *this  *  u  ); 

328  } 

329 

330  Vectors  rotate(  const  Vectors  a,  double  angle  )  {  //  rotates  vector  about  given  axial  vector,  a,  through  the  given 

angle 

331 

332  Vector  a_hat  =  a.unit();  //  unit  vector  along  the  axial  vector 

333  Vector  cross  =  a_hat  *this;  //  cross  product  of  a_hat  with  the  given  vector 

334  return  *this  +=  sin(  angle  )  *  cross  +  (  1.  -  cos(  angle  )  )  *  (  a_hat  ~  cross  ); 

335  } 

336 

337  Vector&  rot(  const  Vectors  a,  double  angle  )  {  //  synonym  for  rotate 
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return  rotate (  a,  angle  ); 


338 

339 

340  } 

341 

342  private: 

343 

344  double  _x,  _y,  _z;  //  cartesian  representation 

345 

346  double  _mag(  void  )  const  {  //  compute  the  magnitude 

347 

348  double  mag  =  sqrt(  _x  *  _x  +  _y  *  _y  +  _z  *  _z  ); 

349  if  (  mag  <  TOL  )  mag  =  0.0L; 

350  return  mag; 

351  > 

352 

353  double  _theta(  void  )  const  {  //  compute  the  polar  angle  (radians) 

354 

355  if  (  -z  !=  0.  )  return  atan2(  sqrt(  _x  *  _x  +  _y  *  _y  ),  _z  ); 

356  else  return  M_PI_2; 

357  > 

358 

359  double  _phi(  void  )  const  {  //  compute  the  azimuthal  angle  (radians) 

360 

361  if  (  _x  !=  0.  ) 

362  return  atan2(  _y,  _x  ); 

363  else  { 

364  if  (  _y  >  0  ) 

365  return  M_PI_2; 

366  else  if  (  _y  ==  0.  ) 

367  return  0.; 

368  else 

369  return  -M_PI_2; 

370  } 

371  > 

372 

373  //  set  cartesian  representation  from  polar 

374  void  _setCartesian(  double  r,  double  theta,  double  phi  )  { 

375 

376  _x  =  r  *  sin(  theta  )  *  cos(  phi  ); 

377  _y  =  r  *  sin(  theta  )  *  sin(  phi  ); 

378  _z  =  r  *  cos(  theta  ); 

379  > 

380 

381  Vector  _set_precision (  void  )  {  //no  more  than  15  digits  of  precision 

382 

383  if  (  fabs(_x)  <  TOL  )  _x  =  0.0L; 

384  if  (  fabs(_y)  <  TOL  )  _y  =  0.0L; 

385  if  (  fabs(_z)  <  TOL  )  _z  =  0.0L; 

386 

387  return  *this; 

388  } 

389  }; 

390  //  declaration  of  friends 

391  Vector  operator+(  const  Vector&  a,  const  Vectors  b  ); 

392  Vector  operator- (  const  Vectors  a,  const  Vectors  b  ); 

393  Vector  operator*(  const  Vectors  v,  double  s  ); 

394  Vector  operator*(  double  s,  const  Vectors  v  ); 

395  Vector  operator/(  const  Vectors  v,  double  s  ); 

3%  double  operator*(  const  Vectors  a,  const  Vectors  b  ); 

397  Vector  operator^  const  Vectors  a,  const  Vectors  b  ); 

398  double  x(  const  Vectors  v  ); 

399  double  y(  const  Vectors  v  ); 

400  double  z(  const  Vectors  v  ); 

401  double  r(  const  Vectors  v  ); 

402  double  theta(  const  Vectors  v  ); 

403  double  phi(  const  Vectors  v  ); 

404  double  mag(  const  Vectors  v  ); 

405  double  scalar(  const  Vectors  v  ); 

406  double  angle(  const  Vectors  a,  const  Vectors  b  ); 

407  Vector  unit(  const  Vectors  v  ); 

408  Vector  normalize(  const  Vectors  v  ); 

409  double  dircos(  const  Vectors  v,  const  compS  i  ); 

410  Vector  proj (  const  Vectors  a,  const  Vectors  b  ); 

411  std : : istreamS  operator»(  std : : istreamS  is,  Vectors  v  ); 

412  std : : ostreamS  operator«(  std :  :ostreamS  os,  const  Vectors  v  ); 

413  }  //  vector  algebra  namespace 

414  #endif 


Notice  that  the  class  is  enclosed  in  a  va  namespace,  so  that  Vectors  are  declared  by 
va : :  Vector.  Table  A-l  provides  a  reference  sheet  for  basic  usage. 
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Table  A-l.  Vector:  A  CH — f-  class  for  3-dimensional  vector  algebra — reference  sheet 


Operation 

Mathematical  notation 

Vector  class 

Definition 

Let  v  be  an  unspecified  vector. 

Vector  v; 

Let  a  be  the  cartesian  vector  (1,2,3). 

Vector  a(l,2,3);  or 

Vector  a(l ,2, 3, CART)  ; 

Let  b  be  the  polar  vector  (r,0,0).a 

Vector  b(r ,th,ph, POLAR) ; 

Input  vector  a 

n/a 

cin  >>  a; 

Output  vector  a 

n/a 

cout  «  a; 

Cartesian  representation 

Let  a  =  xi  +  yj  +  2k. b 

Vector  a(x,y,z);  or 

Vector  a(x,y,z,CART) ; 

Polar  representation 

Let  a  =  (r,  9,  </>).a 

Vector  a(r ,th,ph, POLAR) ; 

Assign  one  vector  to  another 

Let  b  =  a  or 
b  <t=  a 

b  =  a;  or 
b(a)  ; 

Components  of  vector  a 

O' xi  dyi  ® z 

a.x() ,  a.y() ,  a.z()  or 
x(a) ,  y (a) ,  z(a)  or 
a  [X]  ,  a[Y]  ,  a  [Z] 

r,9,<j> 

a.rO,  a.thetaO,  a.phiO 
or  r(a) ,  theta(a) ,  phi(a) 

Direction  cosines 

v  ■  VIMI,  v-j/|M|,  vk/|M| 

v.dircos(X) ;  ...  or 
dircos(v,X) ;  .  . . 

Vector  addition 

c  =  a  +  b 

c  =  a  +  b; 

Addition  assignment 

b  <t=  b  +  a 

b  +=  a; 

Vector  subtraction 

c=  a  —  b 

c  =  a  -  b; 

Subtraction  assignment 

b  <=  b  —  a 

b  -=  a; 

Multiplication  by  a  scalar  s 

b  =  s  a  or 

b  =  as 

b  =  s  *  a;  or 
b  =  a  *  s ; 

Multiplication  assignment 

a  <=  s  a  or 

a  4=  a  s 

a  *=  s ; 

Dot  (scalar)  product 

c  =  a  ■  b 

c  =  a  *  b; 

Cross  (vector)  product 

c  =  a  X  b 

c  =  a  A  b ; 

Negative  of  a  vector 

—v 

-v; 

Norm,  or  magnitude, 
of  a  vector 

INI 

v.normO;  or  norm (v)  ;  or 
v.magO;  or  mag (v)  ;  or 
v.r();  or  v.scalarO  ; 

Angle  between  two  vectors 

e  =  cos  ‘(iwiiwi) 

angle (a,  b) ; 

Normalize  a  vector 

u  =  v/\\v\\ 

u  =  v. normalize () ;c  or 
u  =  normalize (v) ;  or 
u  =  v.unitO  ;  or 
u  =  unit (v) ; 

Projection  of  a  along  b 

(°’m)  M\ 

proj(a,  b)  ;  or  a. proj  (b)  ; 

Rotate  vector  a  about  the 
axial  vector  u  through  the 
angle  9 

a  +  u  X  a  sin  9  +  u  X  [u  X  a)(l  —  cos6*) 

a.rotate(u,  theta);  or 
a.rot(u,  theta); 

ar  is  the  magnitude,  9  is  the  polar  angle  measured  from  the  2-axis,  and  4>  is  the  azimuthal  angle  measured 
from  the  a:-axis  to  the  plane  that  contains  the  vector  and  the  2-axis.  The  angle  9  and  </>  are  in  radians.  Use 
rad  (deg)  to  convert  degrees  to  radians  and  deg  (rad)  to  convert  radians  to  degrees. 
bi,  j,  and  k  are  unit  vectors  along  the  x-axis,  y-axis,  and  2-axis,  respectively. 

cnormalize  does  not  change  the  vector  it  is  invoked  on;  it  merely  returns  the  vector  divided  by  its  norm. 
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Intentionally  left  blank. 
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Appendix  B.  Rotation  Class 
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Listing  B-l.  Rotation.h 


1 

//  Rotation.h:  Rotation  class  definition  for  the  algebra  of  3D  rotations 

2 

//  Ref:  Kuipers,  J.  B.,  Quaternions  and  Rotation  Sequences,  Princeton,  1999. 

3 

//  Altmann,  S.  L.,  Rotations,  Quaternions,  and  Double  Groups,  1986. 

4 

//  Doran  &  Lasenby,  Geometric  Algebra  for  Physicists,  Cambridge,  2003. 

5 

//  R.  Saucier,  March  2005  (Last  revised  June  2016) 

6 

7 

#ifndef  ROTATION.H 

8 

#define  ROTATION.H 

9 

10 

#include  "Vector. h" 

11 

#include  "Random. h" 

12 

#include  <iostream> 

13 

14 

namespace  va  {  //  vector  algebra  namespace 

15 

16 

const  Vector  DEFAULT.UNIT. VECTOR (  0.,  0.,  1.  );  //  arbitrarily  choose  k 

17 

const  double  DEFAULT_ROTATION_ANGLE(  0.  );  //  arbitrarily  choose  0 

18 

const  double  TW0_PI(  2.  *  M_PI  ); 

19 

20 

enum  ORDER  {  //  order  of  rotation  sequence  about  body  axes 

21 

22 

//  six  distinct  principal  axes  factorizations 

23 

ZYX,  //  first  about  z-axis,  second  about  y-axis  and  third  about  x-axis 

24 

XYZ,  //  first  about  x-axis,  second  about  y-axis  and  third  about  z-axis 

25 

YXZ,  //  first  about  y-axis,  second  about  x-axis  and  third  about  z-axis 

26 

ZXY,  //  first  about  z-axis,  second  about  x-axis  and  third  about  y-axis 

27 

XZY,  //  first  about  x-axis,  second  about  z-axis  and  third  about  y-axis 

28 

YZX,  //  first  about  y-axis,  second  about  z-axis  and  third  about  x-axis 

29 

30 

//  six  repeated  principal  axes  factorizations 

31 

ZYZ,  //  first  about  z-axis,  second  about  y-axis  and  third  about  z-axis 

32 

ZXZ,  //  first  about  z-axis,  second  about  x-axis  and  third  about  z-axis 

33 

YZY,  //  first  about  y-axis,  second  about  z-axis  and  third  about  y-axis 

34 

YXY,  //  first  about  y-axis,  second  about  x-axis  and  third  about  y-axis 

35 

XYX,  //  first  about  x-axis,  second  about  y-axis  and  third  about  x-axis 

36 

XZX  //  first  about  x-axis,  second  about  z-axis  and  third  about  x-axis 

37 

}; 

38 

39 

struct  quaternion  {  //  q  =  w  +  v,  where  w  is  scalar  part  and  v  is  vector  part 

40 

41 

quaternion (  void  )  { 

42 

} 

43 

44 

quaternion (  double  scalar.  Vector  vector  )  :  w(  scalar  ),  v(  vector  )  { 

45 

> 

46 

47 

-quaternion (  void  )  { 

48 

> 

49 

50 

//  overloaded  multiplication  of  two  quaternions 

51 

friend  quaternion  operator*(  const  quaternions  ql,  const  quaternions  q2  )  { 

52 

53 

return  quaternion ( 

54 

(  ql.w  *  q2.w  )  -  (  ql.v  *  q2.v  ),  //  scalar  part 

55 

(  ql.w  *  q2.v  )  +  (  q2.w  *  ql.v  )  +  (  ql.v  /'  q2.v  )  //  vector  part 

56 

); 

57 

} 

58 

59 

double  w;  //  scalar  part 

60 

Vector  v;  //  vector  part  (actually  a  bivector  disguised  as  a  vector) 

61 

}; 

62 

63 

struct  sequence  {  //  rotation  sequence  about  three  principal  body  axes 

64 

65 

sequence (  void  )  { 

66 

} 

67 

68 

sequence(  double  phi_l,  double  phi_2,  double  phi_3  )  :  first(  phi_l  ),  second(  phi_2  ) 

third (  phi_3  )  { 

69 

} 

70 

71 

-sequence (  void  )  { 

72 

> 

73 

74 

//  factor  quaternion  into  Euler  rotation  sequence  about  three  distinct  principal  axes 

75 

sequence  factor(  const  quaternions  p,  ORDER  order  )  { 

76 

77 

double  p0  =  p.w; 

78 

double  pi  =  x(  p.v  ) ; 

79 

double  p2  =  y(  p.v  ) ; 

80 

double  p3  =  z(  p.v  ) ; 

81 

82 

double  phi_l,  phi_2,  phi_3; 

83 

if  (  order  ==  ZYX  )  {  //  distinct  principal  axes  zyx 
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84 

85 

double  A  =  p0  *  pi  +  p2  *  p3; 

86 

double  B  =  (  p2  -  p0  )  *  (  p2  +  p0  ) ; 

87 

double  D  =  (  pi  -  p3  )  *  (  pi  +  p3  ) ; 

88 

89 

phi_3  =  atari (  -  2.  *  A  /  (  B  +  D  )  ); 

90 

double  c0  =  cos(  0.5  *  phi_3  ); 

91 

double  cl  =  sin(  0.5  *  phi_3  ); 

92 

93 

double  q0  =  p0  *  c0  +  pi  *  cl; 

94 

//double  ql  =  pi  *  c0  -  p0  *  cl; 

95 

double  q2  =  p2  *  c0  -  p3  *  cl; 

96 

double  q3  =  p3  *  c0  +  p2  *  cl; 

97 

98 

phi_l  =  2.  *  atan(  q3  /  q0  ); 

99 

phi_2  =  2.  *  atari (  q2  /  q0  ) ; 

100 

} 

101 

else  if  (  order  ==  XYZ  )  {  //  distinct 

principal 

axes 

xyz 

102 

103 

double  A  =  pi  *  p2  -  p0  *  p3; 

104 

double  B  =  (  pi  -  p3  )  *  (  pi  +  p3  ) ; 

105 

double  D  =  (  p0  -  p2  )  *  (  p0  +  p2  ) ; 

106 

107 

phi_3  =  atan(  -  2.  *  A  /  (  B+D  )  ); 

108 

double  c0  =  cos(  0.5  *  phi_3  ); 

109 

double  c3  =  sin(  0.5  *  phi_3  ); 

110 

111 

double  q0  =  p0  *  c0  +  p3  *  c3; 

112 

double  ql  =  pi  *  c0  -  p2  *  c3; 

113 

double  q2  =  p2  *  c0  +  pi  *  c3; 

114 

//double  q3  =  p3  *  c0  -  p0  *  c3; 

115 

116 

phi_l  =  2.  *  atan(  ql  /  q0  ) ; 

117 

phi_2  =  2.  *  atan(  q2  /  q0  ) ; 

118 

} 

119 

else  if  (  order  ==  YXZ  )  {  //  distinct 

principal 

axes 

yxz 

120 

121 

double  A  =  pi  *  p2  +  p0  *  p3; 

122 

double  B  =  pi  *  pi  +  p3  *  p3; 

123 

double  D  =  - (  p0  *  p0  +  p2  *  p2  ) ; 

124 

125 

phi_3  =  atan(  -2.  *A/(B+D)); 

126 

double  c0  =  cos(  0.5  *  phi_3  ); 

127 

double  c3  =  sin(  0.5  *  phi_3  ); 

128 

129 

double  q0  =  p0  *  c0  +  p3  *  c3; 

130 

double  ql  =  pi  *  c0  -  p2  *  c3; 

131 

double  q2  =  p2  *  c0  +  pi  *  c3; 

132 

//double  q3  =  p3  *  c0  -  p0  *  c3; 

133 

134 

phi_l  =  2.  *  atan(  q2  /  q0  ) ; 

135 

phi_2  =  2.  *  atan(  ql  /  q0  ) ; 

136 

} 

137 

else  if  (  order  ==  ZXY  )  {  //  distinct 

principal 

axes 

zxy 

138 

139 

double  A  =  pi  *  p3  -  p0  *  p2; 

140 

double  B  =  (  p0  -  pi  )  *  (  p0  +  pi  ) ; 

141 

double  D  =  (  p3  -  p2  )  *  (  p3  +  p2  ) ; 

142 

143 

phi_3  =  atan(  -2.  *  A  /  (  B  +  D  )  ); 

144 

double  c0  =  cos(  0.5  *  phi_3  ); 

145 

double  c2  =  sin(  0.5  *  phi_3  ); 

146 

147 

double  q0  =  p0  *  c0  +  p2  *  c2; 

148 

double  ql  =  pi  *  c0  +  p3  *  c2; 

149 

//double  q2  =  p2  *  c0  -  p0  *  c2; 

150 

double  q3  =  p3  *  c0  -  pi  *  c2; 

151 

152 

phi_l  =  2.  *  atan(  q3  /  q0  ) ; 

153 

phi_2  =  2.  *  atan(  ql  /  q0  ) ; 

154 

} 

155 

else  if  (  order  ==  XZY  )  {  //  distinct 

principal 

axes 

xzy 

156 

157 

double  A  =  pi  *  p3  +  p0  *  p2; 

158 

double  B  =  - (  p0  *  p0  +  pi  *  pi  ) ; 

159 

double  D  =  p2  *  p2  +  p3  *  p3; 

160 

161 

phi_3  =  atan(  -2.  *  A  /  (  B  +  D  )  ); 

162 

double  c0  =  cos(  0.5  *  phi_3  ); 

163 

double  c2  =  sin(  0.5  *  phi_3  ); 

164 

165 

double  q0  =  p0  *  c0  +  p2  *  c2; 

166 

double  ql  =  pi  *  c0  +  p3  *  c2; 

167 

//double  q2  =  p2  *  c0  -  p0  *  c2; 

168 

double  q3  =  p3  *  c0  -  pi  *  c2; 
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169 


170 

phi_l  =  2.  *  atan(  ql  /  q0  ) ; 

171 

phi_2  =  2.  *  atan(  q3  /  q0  ) ; 

172 

} 

173 

else  if  (  order  ==  YZX  )  {  //  distinct 

principal 

axes 

yzx 

174 

175 

double  A  =  p2  *  p3  -  p0  *  pi; 

176 

double  B  =  p0  *  p0  +  p2  *  p2; 

177 

double  D  =  - (  pi  *  pi  +  p3  *  p3  ) ; 

178 

179 

phi_3  =  atan(  -2.  *  A  /  (  B  +  D  )  ); 

180 

double  c0  =  cos(  0.5  *  phi_3  ); 

181 

double  cl  =  sin(  0.5  *  phi_3  ); 

182 

183 

double  q0  =  p0  *  c0  +  pi  *  cl; 

184 

//double  ql  =  pi  *  c0  -  p0  *  cl; 

185 

double  q2  =  p2  *  c0  -  p3  *  cl; 

186 

double  q3  =  p3  *  c0  +  p2  *  cl; 

187 

188 

phi_l  =  2.  *  atan(  q2  /  q0  ) ; 

189 

phi_2  =  2.  *  atan(  q3  /  q0  ) ; 

190 

} 

191 

else  if  (  order  ==  ZYZ  )  {  //  repeated 

principal 

axes 

zyz 

192 

193 

double  A  =  p0  *  pi  +  p2  *  p3; 

194 

double  B  =  -2.  *  p0  *  p2; 

195 

double  D  =  2.  *  pi  *  p3; 

1% 

197 

phi_3  =  atan(  -2.  *  A  /  (  B  +  D  )  ); 

198 

double  c0  =  cos(  0.5  *  phi_3  ); 

199 

double  c3  =  sin(  0.5  *  phi_3  ); 

200 

201 

double  q0  =  p0  *  c0  +  p3  *  c3; 

202 

//double  ql  =  pi  *  c0  -  p2  *  c3; 

203 

double  q2  =  p2  *  c0  +  pi  *  c3; 

204 

double  q3  =  p3  *  c0  -  p0  *  c3; 

205 

206 

phi_l  =  2.  *  atan(  q3  /  q0  ) ; 

207 

phi_2  =  2.  *  atan(  q2  /  q0  ) ; 

208 

} 

209 

else  if  (  order  ==  ZXZ  )  {  //  repeated 

principal 

axes 

ZXZ 

210 

211 

double  A  =  p0  *  p2  -  pi  *  p3; 

212 

double  B  =  2.  *  p0  *  pi; 

213 

double  D  =  2.  *  p2  *  p3; 

214 

215 

phi_3  =  atan(  -2.  *  A  /  (  B  +  D  )  ); 

216 

double  c0  =  cos(  0.5  *  phi_3  ); 

217 

double  c3  =  sin(  0.5  *  phi_3  ); 

218 

219 

double  q0  =  p0  *  c0  +  p3  *  c3; 

220 

double  ql  =  pi  *  c0  -  p2  *  c3; 

221 

//double  q2  =  p2  *  c0  +  pi  *  c3; 

222 

double  q3  =  p3  *  c0  -  p0  *  c3; 

223 

224 

phi_l  =  2.  *  atan(  q3  /  q0  ) ; 

225 

phi_2  =  2.  *  atan(  ql  /  q0  ) ; 

226 

} 

227 

else  if  (  order  ==  YZY  )  {  //  repeated 

principal 

axes 

yzy 

228 

229 

double  A  =  p0  *  pi  -  p2  *  p3; 

230 

double  B  =  p0  *  p3  +  pi  *  p2; 

231 

232 

phi_3  =  atan(  -A  /  B  ) ; 

233 

double  c0  =  cos(  0.5  *  phi_3  ); 

234 

double  c2  =  sin(  0.5  *  phi_3  ); 

235 

236 

double  q0  =  p0  *  c0  +  p2  *  c2; 

237 

//double  ql  =  pi  *  c0  +  p3  *  c2; 

238 

double  q2  =  p2  *  c0  -  p0  *  c2; 

239 

double  q3  =  p3  *  c0  -  pi  *  c2; 

240 

241 

phi_l  =  2.  *  atan(  q2  /  q0  ) ; 

242 

phi_2  =  2.  *  atan(  q3  /  q0  ) ; 

243 

} 

244 

else  if  (  order  ==  YXY  )  {  //  repeated 

principal 

axes 

yxy 

245 

246 

double  A  =  p0  *  p3  +  pi  *  p2; 

247 

double  B  =  -2.  *  p0  *  pi; 

248 

double  D  =  2.  *  p2  *  p3; 

249 

250 

phi_3  =  atan(  -2.  *  k  /  (  B+D  )  ); 

251 

double  c0  =  cos(  0.5  *  phi_3  ); 

252 

double  c2  =  sin(  0.5  *  phi_3  ); 

253 
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254 

double  q0  =  p0  *  c0  +  p2  *  c2; 

255 

double  ql  =  pi  *  c0  +  p3  *  c2; 

256 

double  q2  =  p2  *  c0  -  p0  *  c2; 

257 

//double  q3  =  p3  *  c0  -  pi  *  c2; 

258 

259 

phi_l  =  2.  *  atan(  q2  /  q0  ); 

260 

phi_2  =  2.  *  atan(  ql  /  q0  ); 

261 

} 

262 

else  if  (  order  ==  XYX  )  {  //  repeated  principal  axes  xyx 

263 

264 

double  A  =  p0  *  p3  -  pi  *  p2; 

265 

double  B  =  p0  *  p2  +  pi  *  p3; 

266 

267 

phi_3  =  atan(  -A  /  B  ) ; 

268 

double  c0  =  cos(  0.5  *  phi_3  ); 

269 

double  cl  =  sin(  0.5  *  phi_3  ); 

270 

271 

double  q0  =  p0  *  c0  +  pi  *  cl; 

272 

double  ql  =  pi  *  c0  -  p0  *  cl; 

273 

double  q2  =  p2  *  c0  -  p3  *  cl; 

274 

//double  q3  =  p3  *  c0  +  p2  *  cl; 

275 

276 

phi_l  =  2.  *  atan(  ql  /  q0  ); 

277 

phi_2  =  2.  *  atan(  q2  /  q0  ) ; 

278 

} 

279 

else  if  (  order  ==  XZX  )  {  //  repeated  principal  axes  xzx 

280 

281 

double  A  =  p0  *  p2  +  pi  *  p3; 

282 

double  B  =  -p0  *  p3  +  pi  *  p2; 

283 

284 

phi_3  =  atan(  -A  /  B  ) ; 

285 

double  c0  =  cos(  0.5  *  phi_3  ); 

286 

double  cl  =  sin(  0.5  *  phi_3  ); 

287 

288 

double  q0  =  p0  *  c0  +  pi  *  cl; 

289 

double  ql  =  pi  *  c0  -  p0  *  cl; 

290 

//double  q2  =  p2  *  c0  -  p3  *  cl; 

291 

double  q3  =  p3  *  c0  +  p2  *  cl; 

292 

293 

phi_l  =  2.  *  atan(  ql  /  q0  ); 

294 

phi_2  =  2.  *  atan(  q3  /  q0  ) ; 

295 

} 

296 

else  { 

297 

std::cerr  «  "ERROR  in  Rotation:  invalid  sequence  order: 

"  «  order  «  std::endl; 

298 

exit (  EXIT. FAILURE  ); 

299 

} 

300 

return  sequence!  phi_l,  phi_2,  phi_3  ); 

301 

} 

302 

303 

double  first,  //  about  first  body  axis 

301 

second,  //  about  second  body  axis 

305 

third;  //  about  third  body  axis 

306 

};  //  end  struct  sequence 

307 

308 

struct  matrix  {  //  all  matrices  here  are  rotations  in  three-space 

309 

310 

//  overloaded  multiplication  of  two  matrices 

311 

//  (defined  as  a  convenience  to  the  user;  not  used  in  Rotation 

class) 

312 

friend  matrix  operator*(  const  matrix&  A,  const  matrix&  B  )  { 

313 

314 

matrix  C; 

315 

C.all  =  A. all  *  B . all  +  A.al2  *  B.a21  +  A.al3  *  B.a31; 

316 

C.al2  =  A. all  *  B.al2  +  A.al2  *  B.a22  +  A.al3  *  B.a32; 

317 

C . al3  =  A. all  *  B.al3  +  A.al2  *  B.a23  +  A.al3  *  B.a33; 

318 

319 

C.a21  =  A. a21  *  B.all  +  A.a22  *  B.a21  +  A.a23  *  B.a31; 

320 

C.a22  =  A. a21  *  B.al2  +  A.a22  *  B.a22  +  A.a23  *  B.a32; 

321 

C.a23  =  A. a21  *  B.al3  +  A.a22  *  B.a23  +  A.a23  *  B.a33; 

322 

323 

C.a31  =  A. a31  *  B.all  +  A.a32  *  B.a21  +  A.a33  *  B.a31; 

324 

C.a32  =  A. a31  *  B.al2  +  A.a32  *  B.a22  +  A.a33  *  B.a32; 

325 

C.a33  =  A. a31  *  B.al3  +  A.a32  *  B.a23  +  A.a33  *  B.a33; 

326 

327 

return  C; 

328 

} 

329 

330 

//  transpose  of  a  matrix 

331 

//  (defined  as  a  convenience  to  the  user;  not  used  in  Rotation 

class) 

332 

friend  matrix  transpose(  const  matrix&  A  )  { 

333 

334 

matrix  B; 

335 

B.all  =  A. all; 

336 

B.al2  =  A. a21; 

337 

B. al3  =  A. a31; 

338 
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339  B.a21  =  A.al2 

340  B.a22  =  A.a22 

341  B.a23  =  A.a32 

342 

343  B.a31  =  A.al3 

344  B.a32  =  A.a23 

345  B.a33  =  A.a33 

346 

347  return  B; 

348  } 

349 


350 

//  inverse 

of 

a  matrix 

351 

//  (defined  as  a  convenience 

to  the 

user;  not  used  in  Rotation  class) 

352 

friend  matrix 

inverse  1 

[  const 

matrix&  t 

*  >  t 

353 

354 

double 

det 

=  A. all 

*  (  A. 

a22  *  A. 

a33  -  A. a23 

*  A. a32  )  + 

355 

A.  al2 

*  (  A. 

a23  *  A. 

a31  -  A. a21 

*  A. a33  )  + 

356 

A.  al3 

*  (  A. 

a21  *  A. 

a32  -  A. a22 

*  A. a31  ); 

357 

assert  ( 

det  !=  0. 

] 

11 ; 

358 

matrix 

B; 

359 

B. all  = 

+  ( 

A.  a22 

* 

A.  a33 

-  A. a23 

* 

A. a32  )  / 

det ; 

360 

B.al2  = 

-( 

A.al2 

* 

A.  a33 

-  A. al3 

* 

A. a32  )  / 

det ; 

361 

B. al3  = 

+  ( 

A.  al2 

* 

A.  a23 

-  A. al3 

* 

A. a22  )  / 

det ; 

362 

363 

B.a21  = 

-( 

A.  a21 

* 

A.  a33 

-  A. a23 

* 

A. a31  )  / 

det ; 

364 

B.a22  = 

+  ( 

A. all 

* 

A.  a33 

-  A. al3 

* 

A. a31  )  / 

det ; 

365 

B.a23  = 

-( 

A. all 

* 

A.  a23 

-  A. al3 

* 

A. a21  )  / 

det ; 

366 

367 

B.a31  = 

+  ( 

A.  a21 

* 

A.  a32 

-  A. a22 

* 

A. a31  )  / 

det ; 

368 

B.a32  = 

-( 

A. all 

* 

A.  a32 

-  A. al2 

* 

A. a31  )  / 

det ; 

369 

B.a33  = 

+  ( 

A. all 

* 

A.  a22 

-  A. al2 

* 

A. a21  )  / 

det ; 

370 

371 

return 

B; 

372 

} 

373 

374  //  returns  the  matrix  with  no  more  than  15  decimal  digit  accuracy 


375 

friend 

matrix  set_precision 

const 

matrix&t  A  )  { 

376 

377 

matrix  B(  A  ) ; 

378 

if 

fabs(B.all) 

< 

TOL  ) 

B .  all 

=  0.0L; 

379 

if 

fabs (B . al2) 

< 

TOL  ) 

B .  al2 

=  0.0L; 

380 

if 

fabs(B.al3) 

< 

TOL  ) 

B .  al3 

=  0.0L; 

381 

382 

if 

fabs (B . a21) 

< 

TOL  ) 

B.a21 

=  0.0L; 

383 

if 

fabs (B. a22) 

< 

TOL  ) 

B.a22 

=  0.0L; 

384 

if 

fabs (B . a23) 

< 

TOL  ) 

B.a23 

=  0.0L; 

385 

386 

if 

fabs (B . a31) 

< 

TOL  ) 

B.a31 

=  0.0L; 

387 

if 

fabs (B . a32) 

< 

TOL  ) 

B.a32 

=  0.0L; 

388 

if 

fabs (B . a33) 

< 

TOL  ) 

B.a33 

=  0.0L; 

389 

390  return  B; 

391  > 

392 

393  //  convenient  matrix  properties,  but  not  essential  to  the  Rotation  class 

394 

395  friend  double  tr(  const  matrix&  A  )  {  //  trace  of  a  matrix 

396 

397  return  A, all  +  A.a22  +  A.a33; 

398  } 

399 

400  friend  double  det(  const  matrix&  A  )  {  //  determinant  of  a  matrix 

401 

402  return  A. all  *  (  A.a22  *  A.a33  -  A.a23  *  A.a32  )  + 

403  A. al2  *  (  A.a23  *  A.a31  -  A.a21  *  A.a33  )  + 

404  A. al3  *  (  A.a21  *  A.a32  -  A.a22  *  A.a31  ); 

405  } 

406 

407  friend  Vector  eigenvector  const  matrix&  A  )  {  //  eigenvector  of  a  matrix 

408 

409  return  unit(  (  A.a32  -  A.a23  )  *  Vector(  1.,  0.,  0.  )  + 

410  (  A.al3  -  A.a31  )  *  Vector(  0.,  1.,  0.  )  + 

411  (  A.a21  -  A.al2  )  *  Vector(  0.,  0.,  1.  )  ); 

412  > 

413 

414  friend  double  angle(  const  matrix&  A  )  {  //  angle  of  rotation 

415 

416  return  acos(  0.5  *  (  A. all  +  A.a22  +  A.a33  -  1.  )  ); 

417  } 

418 

419  Vector  eigenvector  void  )  {  //  axis  of  rotation 

420 


return  unit( 

(  a32  • 

■  a23  ) 

i  *  Vector( 

1. , 

,  0. , 

,  0. 

)  + 

(  a  13  • 

■  a31  ) 

i  *  Vector( 

0. , 

,  1. , 

,  0. 

)  + 

(  a21  • 

-  a  12  ) 

i  *  Vector( 

0. , 

,  0.  , 

,  1. 

)  ); 

Approved  for  public  release;  distribution  is  unlimited. 


34 


424  } 

425 

426  double  tr(  void  )  {  //  trace  of  the  matrix 

427 

428  return  all  +  a22  +  a33; 

429  } 

430 

431  double  angle (  void  )  {  //  angle  of  rotation 

432 

433  return  acos(  0.5  *  (  all  +  a22  +  a33  -  1.  )  ); 

434  > 

435 

436  double  all,  al2,  al3,  //  1st  row 

437  a21,  a22,  a23,  //  2nd  row 

438  a31 ,  a32 ,  a33;  //  3rd  row 

439  };  //  end  struct  matrix 

440 

441  class  Rotation  { 

442 

443  //  friends  list 

444 

445  //  overloaded  multiplication  of  two  successive  rotations  (using  quaternions) 

446  //  notice  the  order  is  important;  first  the  right  is  applied  and  then  the  left 

447  friend  Rotation  operator*(  const  Rotations  Rl,  const  Rotations  R2  )  { 

448 

449  return  Rotation(  to_quaternion (  Rl  )  *  to_quaternion (  R2  )  ) ; 

450  } 

451 

452  //  rotation  of  a  vector 

453  //  overloaded  multiplication  of  a  vector  by  a  rotation  (using  quaternions) 

454  friend  Vector  operator*(  const  Rotations  R,  const  Vectors  a  )  { 

455 

456  quaternion  q(  to_quaternion (  R  )  ); 

457  double  w(  q .w  ) ; 

458  Vector  v(  q.v  ) ; 

459 

460  Vector  b  =  2.  *  (  v  A  a  ); 

461  return  a+(w*b)  +  (v/'b); 

462  > 

463 

464  //  spherical  linear  interpolation  on  the  unit  sphere  from  ul  to  u2 

465  friend  Vector  slerp(  const  Vectors  ul,  const  Vectors  u2,  double  theta,  double  t  )  { 

466 

467  assert(  theta  !=  0  ); 

468  assert (  0.  <=  t  SS  t  <=  1.  ); 

469  return  (  sin(  (  1.  -  t  )  *  theta  )  *  ul  +  sin(  t  *  theta  )  *  u2  )  /  sin(  theta  ); 

470  } 

471 

472  //  spherical  linear  interpolation  on  the  unit  sphere  from  ul  to  u2 

473  friend  Vector  slerp(  const  Vector&  ul,  const  Vectors  u2,  double  t  )  { 

474 

475  double  theta  =  angle (  ul,  u2  ); 

476  if  (  theta  ==  0.  )  return  ul; 

477  assert (  0.  <=  t  SS  t  <=  1.  ); 

478  return  (  sin(  (  1.  -  t  )  *  theta  )  *  ul  +  sin(  t  *  theta  )  *  u2  )  /  sin(  theta  ); 

479  > 

480 

481  //  access  functions 

482 

483  //  return  the  unit  axial  vector 

484  friend  Vector  vec(  const  Rotations  R  )  {  //  return  axial  unit  eigenvector 

485 

486  return  R._vec; 

487  > 

488 

489  //  return  the  rotation  angle 

490  friend  double  ang(  const  Rotations  R  )  {  //  return  rotation  angle  (rad) 

491 

492  return  R._ang; 

493  } 

494 

495  //  inverse  rotation 

496  friend  Rotation  inverse(  Rotation  R  )  { 

497 

498  return  Rotation (  R._vec,  -R._ang  ); 

499  > 

500 

501  //  conversion  to  quaternion 

502  friend  quaternion  to_quaternion(  const  Rotations  R  )  { 

503 

504  double  a  =  0.5  *  R._ang; 

505  Vector  u  =  R._vec; 

506 

507  return  quaternion (  cos(  a  ),  u  *  sin(  a  )  ); 

508  > 
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509 

510  //  conversion  to  rotation  matrix 

511  friend  matrix  to_matrix(  const  Rotations  R  )  { 

512 

513  quaternion  q(  to_quaternion (  R  )  ); 

514  double  w  =  q.w; 

515  Vector  v  =  q.v; 

516  double  vl=v[  X  ],  v2=v[  Y  ],  v3=v[  Z  ]; 

517 

518  matrix  A; 


519 

A. all 

= 

2. 

*  i 

(  w  *  w  - 

0.5 

+ 

vl 

*  Ml  ) ; 

// 

1st 

row, 

1st 

col 

520 

A.  al2 

= 

2. 

(  vl  *  v2 

-  w 

* 

v3 

); 

// 

1st 

row, 

2nd 

col 

521 

A.  al3 

= 

2. 

*  i 

(  vl  *  v3 

+  w 

* 

v2 

); 

// 

1st 

row, 

3rd 

col 

522 

523 

A.a21 

= 

2. 

*  i 

(  vl  *  v2 

+  w 

* 

v  3 

); 

// 

2nd 

row, 

1st 

col 

524 

A.a22 

= 

2. 

*  i 

(  W  *  W  - 

0.5 

+ 

m2 

*  v2  ) ; 

// 

2nd 

row, 

2nd 

col 

525 

A.  a23 

= 

2. 

(  v2  *  v3 

-  w 

* 

Ml 

); 

// 

2nd 

row, 

3rd 

col 

526 

527 

A.a31 

= 

2. 

(  vl  *  v3 

-  w 

* 

m2 

); 

// 

3rd 

row, 

1st 

col 

528 

A.  a32 

= 

2. 

*  i 

(  v2  *  v3 

+  w 

* 

Ml 

); 

// 

3rd 

row, 

2nd 

col 

529 

A.  a33 

= 

2. 

(  w  *  w  - 

0.5 

+ 

m3 

*  v3  ); 

// 

3rd 

row, 

3rd 

col 

530 

531  return  A; 

532  > 

533 

534  //  factor  rotation  into  a  rotation  sequence 

535  friend  sequence  factor(  const  Rotations  R,  ORDER  order  )  { 

536 

537  sequence  s; 

538  return  s. factor (  to_quaternion (  R  ),  order  ); 

539  > 

540 

541  //  factor  matrix  representation  of  rotation  into  a  rotation  sequence 

542  friend  sequence  factor(  const  matrixS  A,  ORDER  order  )  { 

543 

544  sequence  s; 

545  return  s. factor (  to_quaternion (  Rotation (  A  )  ),  order  ); 

546  } 

547 

548  //  overloaded  stream  operators 

549 

550  //  input  a  rotation 

551  friend  std : : istreamS  operator»(  std : : istreamS  is,  Rotations  R  )  { 

552 

553  std::cout  «  "Specify  axis  of  rotation  by  entering  an  axial  vector  (need  not  be  a  unit  vector)"  «  std::endl; 

554  is  »  R._vec; 

555  std::cout  «  "Enter  the  angle  of  rotation  (deg): 

556  is  »  R._ang; 

557 

558  R._vec  =  unit(  R._vec  );  //  store  the  unit  vector  representing  the  axis 

559  R._ang  =  R._ang  *  D2R;  //  store  the  rotation  angle  in  radians 

560  return  is; 

561  > 

562 

563  //  output  a  rotation 

564  friend  std : : ostreamS  operator«(  std : : ostreamS  os,  const  Rotations  R  )  { 

565 

566  return  os  «  R._vec  «  "\t"  «  R._ang  *  R2D; 

567  > 

568 

569  //  output  a  quaternion 

570  friend  std :: ostreamS  operator«(  std :: ostreamS  os,  const  quaternions  q  )  { 

571 

572  return  os  «  q.w  «  "\t"  «  q.v; 

573  > 

574 

575  //  output  a  matrix 

576  friend  std :: ostreamS  operator«(  std :: ostreamS  os,  const  matrixS  A  )  { 

577 

578  matrix  B  =  set_precision (  A  );  //no  more  than  15  decimal  digits  of  accuracy 

579  return  os  «  B.all  «  "\t"  «  B.al2  «  "\t"  «  B.al3  «  std::endl 

580  «  B.a21  «  "\t"  «  B.a22  «  "\t"  «  B.a23  «  std::endl 

581  «  B.a31  «  "\t"  «  B.a32  «  "\t"  «  B.a33; 

582  > 

583 

584  public: 

585 

586  //  constructor  from  three  angles  (rad),  phi_l,  phi-2,  phi_3  (in  that  order,  left  to  right) 

587  //  about  three  distinct  principal  body  axes 


588 

Rotation ( 

double  phi_l, 

double 

phi_ 

2,  double  phi 

_3,  ORDER  order  )  { 

589 

590 

double 

ang_l  =  0.5  * 

phi_l , 

cl 

cos(  ang_l  ) 

,  si  =  sin(  ang_l  ) ; 

591 

double 

ang_2  =  0.5  * 

phi_2 , 

c2 

cos(  ang_2  ) 

,  s2  =  sin(  ang_2  ) ; 

592 

double 

ang_3  =  0.5  * 

phi_3 , 

c3 

cos(  ang_3  ) 

,  s3  =  sin(  ang_3  ) ; 

593  double  w; 
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Vector  v; 


594 


595 

596 

if  ( 

order  ==  ZYX  )  {  //  1st  about  z-axis,  2nd 

about  y-axis,  3rd 

about  x-axis  (Aerospace  sequence) 

597 

598 

w 

=  cl  *  c2  *  c3  +  si  *  s2  *  s3; 

599 

V 

=  Vector(  cl  *  c2  *  s3  -  si  * 

s2  *  c3 , 

600 

cl  *  s2  *  c3  +  si  * 

c2  *  s3, 

601 

-cl  *  s2  *  s3  +  si  * 

c2  *  c3  ) ; 

602 

} 

603 

else 

if  (  order  =  XYZ  )  {  //  1st 

about  x-axis 

,  2nd  about  y-axis 

,  3rd  about  z-axis  (FATEPEN 

sequence) 

604 

605 

w 

=  cl  *  c2  *  c3  -  si  *  s2  *  s3; 

606 

V 

=  Vector(  cl  *  s2  *  s3  +  si  * 

c2  *  c3, 

607 

cl  *  s2  *  c3  -  si  * 

c2  *  s3, 

608 

cl  *  c2  *  s3  +  si  * 

s2  *  c3  ) ; 

609 

} 

610 

else 

if  (  order  ==  YXZ  )  {  //  1st 

about  y-axis 

,  2nd  about  x-axis 

,  3rd  about  z-axis 

611 

612 

w 

=  cl  *  c2  *  c3  +  si  *  s2  *  s3; 

613 

V 

=  Vector(  cl  *  s2  *  c3  +  si  * 

c2  *  s3, 

614 

-cl  *  s2  *  s3  +  si  * 

c2  *  c3 , 

615 

cl  *  c2  *  s3  -  si  * 

s2  *  c3  ) ; 

616 

} 

617 

else 

if  (  order  ==  ZXY  )  {  //  1st 

about  z-axis 

,  2nd  about  x-axis 

,  3rd  about  y-axis 

618 

619 

w 

=  cl  *  c2  *  c3  -  si  *  s2  *  s3; 

620 

V 

=  Vector(  cl  *  s2  *  c3  -  si  * 

c2  *  s3, 

621 

cl  *  c2  *  s3  +  si  * 

s2  *  c3 , 

622 

cl  *  s2  *  s3  +  si  * 

c2  *  c3  ) ; 

623 

} 

624 

else 

if  (  order  ==  XZY  )  {  //  1st 

about  x-axis 

,  2nd  about  z-axis 

,  3rd  about  y-axis 

625 

626 

w 

=  cl  *  c2  *  c3  +  si  *  s2  *  s3; 

627 

V 

=  Vector(  -cl  *  s2  *  s3  +  si  * 

c2  *  c3 , 

628 

cl  *  c2  *  s3  -  si  * 

s2  *  c3, 

629 

cl  *  s2  *  c3  +  si  * 

c2  *  s3  ) ; 

630 

} 

631 

else 

if  (  order  =  YZX  )  {  //  1st 

about  y-axis 

,  2nd  about  z-axis 

,  3rd  about  x-axis 

632 

633 

w 

=  cl  *  c2  *  c3  -  si  *  s2  *  s3; 

634 

V 

=  Vector(  cl  *  c2  *  s3  +  si  * 

s2  *  c3 , 

635 

cl  *  s2  *  s3  +  si  * 

c2  *  c3 , 

636 

cl  *  s2  *  c3  -  si  * 

c2  *  s3  ) ; 

637 

} 

638 

else 

if  (  order  ==  ZYZ  )  {  //  Euler  sequence, 

1st  about  z-axis, 

2nd  about  y-axis,  3rd 

about 

z-axis 

639 

640 

w 

=  cl  *  c2  *  c3  -  si  *  c2  *  s3; 

641 

V 

=  Vector(  cl  *  s2  *  s3  -  si  * 

s2  *  c3, 

642 

cl  *  s2  *  c3  +  si  * 

s2  *  s3, 

643 

cl  *  c2  *  s3  +  si  * 

c2  *  c3  ) ; 

644 

} 

645 

else 

if  (  order  ==  ZXZ  )  {  //  Euler  sequence, 

1st  about  z-axis, 

2nd  about  x-axis,  3rd 

about 

z-axis 

646 

647 

w 

=  cl  *  c2  *  c3  -  si  *  c2  *  s3; 

648 

V 

=  Vector(  cl  *  s2  *  c3  +  si  * 

s2  *  s3, 

649 

-cl  *  s2  *  s3  +  si  * 

s2  *  c3 , 

650 

cl  *  c2  *  s3  +  si  * 

c2  *  c3  ) ; 

651 

} 

652 

else 

if  (  order  ==  YZY  )  {  //  Euler  sequence, 

1st  about  y-axis, 

2nd  about  z-axis,  3rd 

about 

y-axis 

653 

654 

w 

=  cl  *  c2  *  c3  -  si  *  c2  *  s3; 

655 

V 

=  Vector(  -cl  *  s2  *  s3  +  si  * 

s2  *  c3, 

656 

cl  *  c2  *  s3  +  si  * 

c2  *  c3 , 

657 

cl  *  s2  *  c3  +  si  * 

s2  *  s3  ) ; 

658 

} 

659 

else 

if  (  order  ==  YXY  )  {  //  Euler  sequence, 

1st  about  y-axis, 

2nd  about  x-axis,  3rd 

about 

y-axis 

660 

661 

w 

=  cl  *  c2  *  c3  -  si  *  c2  *  s3; 

662 

V 

=  Vector(  cl  *  s2  *  c3  +  si  * 

s2  *  S3, 

663 

cl  *  c2  *  s3  +  si  * 

c2  *  c3, 

664 

cl  *  s2  *  s3  -  si  * 

s2  *  c3  ); 

665 

} 

666 

else 

if  (  order  ==  XYX  )  {  //  Euler  sequence, 

1st  about  x-axis, 

2nd  about  y-axis,  3rd 

about 

x-axis 

667 

668 

w 

=  cl  *  c2  *  c3  -  si  *  c2  *  s3; 

669 

V 

=  Vector(  cl  *  c2  *  s3  +  si  * 

c2  *  c3 , 

670 

cl  *  s2  *  c3  +  si  * 

s2  *  s3, 

671 

-cl  *  s2  *  s3  +  si  * 

s2  *  c3  ) ; 

672 

} 

673 

else 

if  (  order  =  XZX  )  {  //  Euler  sequence, 

1st  about  x-axis, 

2nd  about  z-axis,  3rd 

about 

x-axis 

674 

675 

w 

=  cl  *  c2  *  c3  -  si  *  c2  *  s3; 

676 

V 

=  Vector(  cl  *  c2  *  s3  +  si  * 

c2  *  c3 , 

677 

cl  *  s2  *  s3  -  si  * 

s2  *  c3, 

678 

cl  *  s2  *  c3  +  si  * 

s2  *  S3  ); 
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679  } 


680 

else  { 

681 

682 

std::cerr  «  "ERROR  in  Rotation:  invalid  order:  "  «  order  «  std: 

: endl; 

683 

exit (  EXIT_ FAILURE  ); 

684 

} 

685 

if  (  w  >=  1 .  |  |  v  ==  0 .  )  { 

686 

_ang  =  DEFAULT_R0TATI0N_ANGLE ; 

687 

_vec  =  DEFAULT- UNIT. VECTOR; 

688 

} 

689 

else  { 

690 

_ang  =  2.  *  acos (  w  ) ; 

691 

_vec  =  v  /  sqrt(  1.  -w*w); 

692 

} 

693 

_set_angle() ;  //  angle  in  the  range  [-M_PI,  M_PI] 

694 

} 

695 

696 

//  constructor  from  a  rotation  sequence 

697 

Rotation!  const  sequence&  s,  ORDER  order  )  { 

698 

699 

Rotation  R(  s. first,  s. second,  s. third,  order  ); 

700 

701 

_vec  =  vec(  R  );  //  set  the  axial  vector 

702 

_ang  =  ang(  R  );  //  set  the  rotation  angle 

703 

_set_angle() ;  //  angle  in  the  range  [-M_PI,  M_PI] 

704 

> 

705 

706 

//  constructor  from  an  axial  vector  and  rotation  angle  (rad) 

707 

Rotation(  const  Vector&  v,  double  a  )  :  _vec(  v  ),  _ang(  a  )  { 

708 

709 

_vec  =  _vec.unit();  //  store  the  unit  vector  representing  the 

axis 

710 

_set_angle() ;  //  angle  in  the  range  [-M_PI,  M_PI] 

711 

} 

712 

713 

//  constructor  using  sphericalCoord  (of  axial  vector)  and  rotation 

angle 

(rad) 

714 

Rotation (  rng :: sphericalCoord  s,  double  ang  )  { 

715 

716 

_vec  =  Vector (  1. ,  s. theta,  s.phi,  POLAR  );  //  unit  vector 

717 

_ang  =  ang; 

718 

_set_angle() ;  //  angle  in  the  range  [-M_PI,  M_PI] 

719 

} 

720 

721 

//  constructor  from  the  cross  product  of  two  vectors 

722 

//  generate  the  rotation  that,  when  applied  to  vector  a,  will  result  in 

vector  b 

723 

Rotation (  const  Vector&  a,  const  Vector&  b  )  { 

724 

725 

_vec  =  unit(  a  A  b  ) ;  //  unit  vector 

726 

double  s  =  a. unit ()  *  b.unit(); 

727 

if  (  s  >=  1.  ) 

728 

_ang  =  0. ; 

729 

else  if  (  s  <=  -1.  ) 

730 

_ang=  M_PI; 

731 

else 

732 

_ang  =  acos (  s  ) ; 

733 

734 

_set_angle() ;  //  angle  in  the  range  [-M_PI,  M_PI] 

735 

} 

736 

737 

//  constructor  from  unit  quaternion 

738 

Rotation (  const  quaternion&  q  )  { 

739 

740 

double  w  =  q.w; 

741 

Vector  v  =  q.v; 

742 

743 

if  (  w  >=  1 .  |  |  v  ==  0 .  )  { 

744 

_ang  =  DEFAULT_ROTATION_ANGLE ; 

745 

-vec  =  DEFAULT- UNIT. VECTOR; 

746 

} 

747 

else  { 

748 

double  n  =  sqrt(  w*w+v*v);  //  need  to  insure  it's  a 

unit 

quaternion 

749 

w  /=  n; 

750 

v  /=  n; 

751 

_ang  =  2.  *  acos (  w  ) ; 

752 

_vec  =  v  /  sqrt(  1.  -w*w); 

753 

} 

754 

_set_angle() ;  //  angle  in  the  range  [-M_PI,  M_PI] 

755 

} 

756 

757 

//  constructor  from  rotation  matrix 

758 

Rotation!  const  matrix&  A  )  { 

759 

760 

_vec  =  (  A.a32  -  A.a23  )  *  Vector(  1.,  0.,  0.  )  + 

761 

(  A.al3  -  A.a31  )  *  Vector(  0. ,  1.,  0.  )  + 

762 

(  A.a21  -  A.al2  )  *  Vector(  0.,  0.,  1.  ); 

763 

if  (  _vec  ==  0.  )  {  //  then  it  must  be  the  identity  matrix 
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764 

_vec  =  DEFAULT. UNIT_ VECTOR; 

765 

_ang  =  DEFAULT_R0TATI0N_ANGLE ; 

766 

> 

767 

else  { 

768 

_vec  =  _vec.unit() ; 

769 

_ang  =  acos(  0.5  *  (  A. all  +  A.a22  +  A.a33  -  1.  )  ); 

770 

} 

771 

_set_angle() ;  //  angle  in  the  range  M_PI] 

772 

} 

773 

774 

// 

constructor  from  two  sets  of  three  vectors,  where  the  pair  must 

be  related  by  a  pure  rotation 

775 

// 

returns  the  rotation  that  will  take  al  to  bl,  a2  to  b2,  and  a3  to  b3 

776 

// 

Ref:  Micheals,  R.  J.  and  Boult,  T.  E.,  "Increasing  Robustness  in 

Self -Localization  and 

Pose  Estimation , "  online 

paper. 

777 

778 

Rotation(  const  Vector&  al,  const  Vector&  a2,  const  Vectors  a3, 

//  initial  vectors 

779 

const  Vectors  bl,  const  Vectors  b2,  const  Vectors  b3  )  { 

//  rotated  vectors 

780 

781 

assert (  det(  al,  a2,  a3  )  !=  0.  SS  det(  bl,  b2,  b3  )  !=  0.  ); 

//  all  vectors  must  be 

nonzero 

782 

assert (  fabs(  det(  al,  a2,  a3  )  -  det(  bl,  b2,  b3  )  )  <0.001  ); 

//  if  it  doesn't  preserve  volume, 

it's  not  a  pu 

rotation 

783 

784 

if  (  det(  al,  a2,  a3  )  ==  1  )  {  //  these  are  basis  vectors  so 

use  simpler  method  to 

construct  the 

rotation 

785 

786 

matrix  A; 

787 

A. all  =  bl  *  al;  A.al2  =  b2  *  al;  A.al3  =  b3  *  al; 

788 

A.a21  =  bl  *  a2;  A.a22  =  b2  *  a2;  A.a23  =  b3  *  a2; 

789 

A.a31  =  bl  *  a3;  A.a32  =  b2  *  a3;  A.a33  =  b3  *  a3; 

790 

791 

_vec  =  (  A.a32  -  A.a23  )  *  Vector(  1.,  0.,  0.  )  + 

792 

(  A.al3  -  A.a31  )  *  Vector(  0.,  1.,  0.  )  + 

793 

(  A.a21  -  A.al2  )  *  Vector(  0.,  0.,  1.  ); 

794 

if  (  _vec  ==  0.  )  {  //  then  it  must  be  the  identity  matrix 

795 

_vec  =  DEFAULT_UNIT_VECTOR ; 

796 

_ang  =  DEFAULT_R0TATI0N_ANGLE ; 

797 

} 

798 

else  { 

799 

_vec  =  _vec . unit ( ) ; 

800 

_ang  =  acos(  0.5  *  (  A. all  +  A.a22  +  A.a33  -  1.  )  ); 

801 

} 

802 

_set_angle( ) ;  //  angle  in  the  range  [-M_PI,  M_PI] 

803 

} 

804 

else  {  //  use  R.  J.  Micheals'  closed-form  solution  to  the  absolute  orientation  problem 

805 

806 

Vector  cl,  c2,  c3; 

807 

double  aaa,  baa,  aba,  aab,  caa,  aca,  aac; 

808 

double  q02,  q0,  q0ql,  ql,  q0q2,  q2,  q0q3,  q3; 

809 

810 

aaa  =  det(  al,  a2,  a3  ) ; 

811 

baa  =  det(  bl,  a2,  a3  ) ; 

812 

aba  =  det(  al,  b2,  a3  ) ; 

813 

aab  =  det(  al,  a2,  b3  ) ; 

814 

815 

q02  =  fabs(  (  aaa  +  baa  +  aba  +  aab  )  /  (  4.  *  aaa  )  ); 

816 

q0  =  sqrt (  q02  ) ; 

817 

818 

cl  =  Vector(  0.,  bl [ Z] ,  - bl [ Y]  ); 

819 

c2  =  Vector(  0.,  b2[Z],  -b2[Y]  ); 

820 

c3  =  Vector(  0.,  b3[Z],  -b3[Y]  ); 

821 

822 

caa  =  det(  cl,  a2,  a3  ) ; 

823 

aca  =  det(  al,  c2,  a3  ) ; 

824 

aac  =  det(  al,  a2,  c3  ) ; 

825 

826 

q0ql  =  (  caa  +  aca  +  aac  )  /  (  4.  *  aaa  ); 

827 

ql  =  q0ql  /  q0; 

828 

829 

cl  =  Vector(  - bl [Z] ,  0.,  bl [X]  ); 

830 

c2  =  Vector(  -b2[Z],  0.,  b2 [X]  ); 

831 

c3  =  Vector(  -b3[Z],  0.,  b3 [X]  ); 

832 

833 

caa  =  det(  cl,  a2,  a3  ) ; 

834 

aca  =  det(  al,  c2,  a3  ) ; 

835 

aac  =  det(  al,  a2,  c3  ) ; 

836 

837 

q0q2  =  (  caa  +  aca  +  aac  )  /  (  4.  *  aaa  ); 

838 

q2  =  q0q2  /  q0 ; 

839 

840 

cl  =  Vector(  bl[Y],  -bl[X],  0.  ); 

841 

c2  =  Vector(  b2[Y],  -b2[X],  0.  ); 

842 

c3  =  Vector(  b3[Y],  -b3[X],  0.  ); 

843 

844 

caa  =  det(  cl,  a2,  a3  ) ; 

845 

aca  =  det(  al,  c2,  a3  ) ; 

846 

aac  =  det(  al,  a2,  c3  ) ; 
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847 

848  q0q3  =  (  caa  +  aca  +  aac  )  /  (  4.  *  aaa  ); 

849  q3  =  q0q3  /  q0; 

850 

851  //  no  need  to  normalize  since  constructed  to  be  unit  quaternions 

852  double  w(  q0  ) ; 

853  Vector  v(  ql,  q2,  q3  ); 

854 

855  if  (  w  >=  1 .  |  |  v  ==  0 .  )  { 

856  _ang  =  DEFAULT_R0TATI0N_ANGLE ; 

857  _vec  =  DEFAULT_UNIT_VECTOR; 

858  } 

859  else  { 

860  _ang  =  2.  *  acos(  w  ); 

861  _vec  =  v  /  sqrt(  1.  -  w  *  w  ); 

862  } 

863  _set_angle( ) ;  //  angle  in  the  range  [-M_PI,  M_PI] 

864  } 

865  } 

866 

867  //  constructor  for  a  uniform  random  rotation,  uniformly  distributed  over  the  unit  sphere, 

868  //  by  fast  generation  of  random  quaternions,  uniformly-distributed  over  the  4D  unit  sphere 

869  //  Ref:  Shoemake,  K.,  "Uniform  Random  Rotations,"  Graphic  Gems  III,  September,  1991. 

870  Rotation!  rng :: Randoms  rng  )  {  //  random  rotation  in  canonical  form 

871 

872  double  s  =  rng. uniform!  0.,  1.  ); 

873  double  si  =  sqrt(  1.  -  s  ); 

874  double  thl  =  rng. uniform!  0-,  TW0.PI  ); 

875  double  x  =  si  *  sin(  thl  ); 

876  double  y  =  si  *  cos(  thl  ); 

877  double  s2  =  sqrt(  s  ); 

878  double  th2  =  rng. uniform!  0.,  TW0_PI  ); 

879  double  z  =  s2  *  sin(  th2  ); 

880  double  w  =  s2  *  cos(  th2  ); 

881  Vector  v(  x,  y,  z  ) ; 

882 

883  if  (  w  >=  1 .  |  |  v  ==  0 .  )  { 

884  _ang  =  DEFAULT_ROTATION_ANGLE ; 

885  _vec  =  DEFAULT. UNIT. VECTOR; 

886  } 

887  else  { 

888  _ang  =  2.  *  acos(  w  ); 

889  _vec  =  v  /  sqrt(  1.  -  w  *  w  ); 

890  } 

891  _set_angle() ;  //  angle  in  the  range  [-M_PI,  M_PI] 

892  > 

893 

894  //  default  constructor 

895  Rotation!  void  )  { 

896 

897  _vec  =  DEFAULT_UNIT_VECTOR; 

898  _ang  =  DEFAULT_ROTATION_ANGLE ; 

899  > 

900 

901  //  default  destructor 

902  -Rotation!  void  )  { 

903  } 

904 

905  //  copy  constructor 

906  Rotation!  const  Rotations  r  )  :  _vec(  r._vec  ),  _ang(  r._ang  )  { 

907 

908  _set_angle() ;  //  angle  in  the  range  [-M_PI,  M-PI] 

909  } 

910 

911  //  overloaded  assignment  operator 

912  Rotations  operator=(  const  Rotations  R  )  { 

913 

914  if  (  this  !=  SR  )  { 

915  _vec  =  R._vec; 

916  _ang  =  R._ang; 

917  _set_angle( ) ;  //  angle  in  the  range  [-M_PI,  M_PI] 

918  } 

919  return  *this; 

920  > 

921 

922  //  conversion  operator  to  return  the  eigenvector  vec 

923  operator  Vector!  void  )  const  { 

924 

925  return  _vec; 

926  } 

927 

928  //  conversion  operator  to  return  the  angle  of  rotation  about  the  eigenvector 

929  operator  double!  void  )  const  { 

930 

931  return  _ang; 
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932 

} 

933 

934 

//  overloaded  arithmetic  operators 

935 

936 

//  inverse  rotation 

937 

Rotation  operator- (  void  )  { 

938 

939 

return  Rotation (  -_vec,  _ang  ); 

940 

} 

941 

942 

//  triple  scalar  product,  same  as  a  *  (  b  ^  c  ) 

943 

inline  double  det(  const  Vectors  a,  const  Vectors  b,  const  Vectors  c  ) 

{ 

944 

945 

return  a [X]  *  (  b[Y]  *  c[Z]  -  b[Z]  *  c[Y]  )  + 

946 

a [ Y]  *  (  b[Z]  *  c[X]  -  b [X]  *  c[Z]  )  + 

947 

a[Z]  *  (  b[X]  *  c[Y]  -  b [Y]  *  c [X]  ); 

948 

> 

949 

950 

private: 

951 

952 

inline  void  _set_angle(  void  )  {  //  always  choose  the  smaller  of  the 

two  angles 

953 

954 

if  (  _ang  >  +TW0_PI  )  _ang  -=  TW0_PI; 

955 

if  (  _ang  <  -TWCLPI  )  _ang  +=  TW0_PI; 

956 

if  (  _ang  >  M_PI  )  { 

957 

_ang  =  TW0_PI  -  _ang; 

958 

_vec  =  -_vec; 

959 

} 

960 

else  if  (  _ang  <  -M_PI  )  { 

961 

_ang  =  TW0_PI  +  _ang; 

962 

_vec  =  -_vec; 

963 

} 

964 

> 

965 

966 

Vector  _vec;  //  unit  eigenvector  representing  the  axis  of  rotation 

967 

double  _ang;  //  angle  of  rotation  (rad)  falls  in  the  range  [-M_PI,  M_PI] 

968  }; 

969 

970 

// 

declaration  of  friends 

971 

quaternion  operator*(  const  quaternions  ql,  const  quaternions  q2  ); 

// 

product  of  two  quaternions 

972 

matrix  operator*(  const  matrixS  A,  const  matrixS  B  ); 

// 

product  of  two  matrices,  first  B, 

then 

973 

A 

matrix  transpose (  const  matrixS  A  ); 

// 

transpose  of  a  matrix 

974 

matrix  inverse(  const  matrixS  A  ); 

// 

inverse  of  a  matrix 

975 

matrix  set_precision(  const  matrixS  A  ); 
decimal  digit  accuracy 

// 

returns  a  matrix  with  no  more  than 

15 

976 

double  tr(  const  matrixS  A  ); 

// 

trace  of  a  matrix 

977 

double  det(  const  matrixS  A  ); 

// 

determinant  of  a  matrix 

978 

Vector  eigenvector  const  matrixS  A  ); 

// 

eigenvector  of  a  rotation  matrix 

979 

double  angle(  const  matrixS  A  ); 

// 

angle  of  rotation  (rad) 

980 

Rotation  operator*(  const  Rotations  Rl,  const  Rotations  R2  ); 

// 

successive  rotations,  first  right, 

981 

then  left 

Vector  operator*(  const  Rotations  R,  const  Vectors  a  ); 

// 

rotation  of  a  vector 

982 

Vector  slerp(  const  Vectors  ul,  const  Vectors  u2,  double  t  ); 

// 

spherical  linear  interpolation  on 

the 

983 

unit  sphere 

Vector  slerp(  const  Vectors  ul,  const  Vectors  u2,  double  theta,  double 
vectors 

t  );  // 

slerp,  given  the  angle  between  the 

984 

Vector  vec(  const  Rotations  R  ); 

// 

return  axial  unit  eigenvector 

985 

double  ang(  const  Rotations  R  ); 

// 

return  rotation  angle  (rad) 

986 

Rotation  inverse(  Rotation  R  ); 

// 

inverse  rotation 

987 

quaternion  to_quaternion (  const  Rotations  R  ); 

// 

convert  rotation  to  a  quaternion 

988 

matrix  to_matrix(  const  Rotations  R  ); 

// 

convert  a  rotation  to  a  rotation 

989 

matrix 

sequence  factor(  const  Rotations  R,  ORDER  order  ); 
sequence 

// 

factor  a  rotation  into  a  rotation 

990 

sequence  factor(  const  matrixS  A,  ORDER  order  ); 

// 

factor  a  rotation  matrix  into  a 

991 

rotation  sequence 

std :: ist reams  operator»(  std :: ist reams  is,  Rotations  R  ); 

// 

input  rotation 

992 

std : : ostreamS  operator«(  std : : ostreamS  os,  const  Rotations  R  ); 

// 

output  a  rotation 

993 

std : : ostreamS  operator«(  std :: ostreamS  os,  const  quaternions  q  ); 

// 

output  a  quaternion 

994 

std :: ostreamS  operator«(  std :: ostreamS  os,  const  matrixS  A  ); 

// 

output  a  rotation  matrix 

995 

996 

} 

//  vector  algebra  namespace 

997 

#endif 

The  Rotation  class  is  also  enclosed  in  a  va  namespace,  so  that  Rotations  are  declared 
by  va:  : Rotation  or  by  the  declaration  using  namespace  va.  Table  B-l  provides  a 
reference  sheet  for  basic  usage. 
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Table  B-l.  Rotation:  A  C-| — K  class  for  3-dimensional  rotations — reference  sheet 


Operation 

Mathematical  notation 

Rotation  class 

Definition3, 

Let  R  be  an  unspecified  rotation. 

Rotation  R; 

Let  R  be  a  rotation  specified  by 
yaw,  pitch,  and  roll.b 

Rotation  R(y ,p,r ,ZYX) ; 

Let  R  be  the  rotation  specified  by 
three  angles,  (f> i,  </>2,  <p3 
applied  in  the  order  x-y-z. 

Rotation  b(phl,ph2,ph3,XYZ) ; 

Let  Ra(a)  be  the  rotation  about 
the  vector  a  through  the  angle  a. 

Vector  a; 

Rotation  R(a, alpha)  ; 

Let  R  be  the  rotation  about  the 
direction  specified  by  the  angles 
{9, 4>)  through  the  angle  a. 

pair<double , double>  p(th,ph) ; 
Rotation  R(p, alpha); 

Let  R  be  the  rotation  specified  by 
the  vector  cross  product  a  x  b. 

Vector  a,  b; 

Rotation  R(a,b) ; 

Let  R  be  the  rotation  that  maps  the 
set  of  linearly  independent  vectors  a,; 
to  the  set  b;,  where  i  =  1,2, 3. 

Vector  al , a2 , a3,bl ,b2 ,b3; 
Rotation  R(al ,a2,a3,bl ,b2,b3) ; 

Let  R  be  the  rotation  specified  by 
the  (unit)  quaternion  q.c 

quaternion  q; 

Rotation  R(q) ; 

Let  R  be  the  rotation  specified  by 
the  3x3  rotation  matrix  Aij.d 

matrix  A; 

Rotation  R(A) ; 

Let  R  be  a  random  rotation,  designed 
to  randomly  orient  any  vector 
uniformly  over  the  unit  sphere. 

rng : : Random  rng ; 

R(rng) ; 

Input  a  rotation  R 

n/a 

cin  >>  R; 

Output  the  rotation  R 

n/a 

cout  <<  R; 

Assign  one  rotation  to 
another 

Let  R.2  =  R\  or 
f?2  <=  R\ 

R2  =  R1 ;  or 

R2(R1) ; 

Product  of  two  successive 
rotations® 

R2R1 

R2  *  R1 ; 

Rotation  of  a  vector  a 

Ra 

R  *  a; 

Inverse  rotation 

~lr1 

inverse (R) ;  or  -R; 

Convert  a  rotation  to  a 
quaternion 

If  Ru{9)  is  the  rotation,  then 
q  =  cos(0/2)  +  wsin(0/2). 

to_quaternion(R) ; 

Convert  a  rotation  to  a 
3x3  matrix 

See  description  on  next  page. 

to_matrix(R) ; 

Factor  a  rotation  into  a 
rotation  sequence 

See  description  on  next  page. 

sequence  s  =  factor (R, ZYX) ;* 

Unit  vector  along  the 
axis  of  rotation 

Unit  vector  u  in  the  rotation  Ra(9) 

Vector (R) ;  or 
vec (R) ; 

Rotation  angle 

Angle  9  in  the  rotation  Rn{9) 

double (R);  or 
ang(R) ; 

aA  rotation  is  represented  in  the  Rotation  class  by  the  pair  (tt,  0),  where  u  is  the  unit  vector  along  the 
axis  of  rotation,  and  9  is  the  counterclockwise  rotation  angle. 

bThe  order  is  significant:  first  yaw  is  applied  as  a  counterclockwise  (CCW)  rotation  about  the  2-axis, 
then  pitch  is  applied  as  a  CCW  rotation  about  the  y'-axis,  and  finally,  roll  is  applied  as  a  CCW  rotation 
about  the  a/'-axis.  The  coordinate  system  is  constructed  from  the  local  tangent  plane  in  which  the  2-axis 
points  toward  earth  center,  the  axaxis  points  along  the  direction  of  travel,  and  the  j/-axis  points  to  the  right, 
to  form  a  right-handed  coordinate  system.  The  particular  order  is  specified  by  using  ZYX.  There  are  a  total 
of  12  possible  orderings  available  to  the  user,  6  of  them  have  distinct  principal  rotation  axes:  XYZ,  XZY, 
YXZ ,  YZX ,  ZXY,  ZYX;  and  6  have  repeated  principal  rotation  axes:  XYX,  XZX,  YXY,  YZY,  ZXZ,  ZYZ. 
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Convert  rotation  to  a  3  x  3  matrix:  First  we  convert  the  rotation  Rn(9)  into  the  unit  quaternion,  via 
q  =  cos((9/2)  +  itsin(0/2),  and  set  w  =  cos(0/2),  the  scalar  part,  and  v  =  ■usin(0/2),  the  vector  part.  Then 
the  rotation  matrix  is 

2  w2  —  1  +  2vf  2v\V2  —  2wv3  2v\V3  +  2wv-i 
2v\V2  +  2wv3  2  w2  —  1  +  2v\  2v2V3  —  2wvi 

2viv3  —  2wv2  2v2V3  +  2wv\  2  w2  —  1  +  2v§ 

Factor  a  rotation  into  a  rotation  sequence:  First  we  convert  the  rotation  Ru(9)  into  the  unit  quaternion, 
via  q  =  cos(0/2)  +  «sin(0/2),  and  set  w  =  cos(0/2),  the  scalar  part,  and  v  =  wsin(0/2),  the  vector  part. 
Next,  let  p0  =  w,  pi  =  v\,  p2  =  v2,  p3  =  ^3  and  set  A  =  p0p  1  +  p2p3,  B  =  p\  -  pi,  D  =  p\  -  p§. 
Then  <j> 3  =  tan-1  (—2 A/{B  +  D ))  is  the  third  angle,  which  is  roll  about  the  axaxis  in  this  case.  Now 
set  c0  =  cos(</>3/2),  ci  =  sin(03/2),  q0  =  p0c0  +  P1C1,  q2  =  P2C0  -  p3ci,  and  q3  =  p3c0  +  p2c\.  Then 
(f>i  =  2tan_1(g3/g0)  is  the  first  angle,  which  is  yaw  about  the  2-axis,  and  <p2  =  2tan_1(g2/9o)  is  the  second 
angle,  which  is  pitch  about  the  y-axis. 


CA  quaternion  is  defined  in  the  Rotation  class  as  follows: 
struct  quaternion  { 
double  w;  //  scalar  part 
Vector  v;  //  vector  part 
}; 

A  unit  quaternion  requires  that  w2  +  ||u||  =  1. 

dA  matrix  is  defined  in  the  Rotation  class  as  follows: 

struct  matrix  { 

double  all,  al2,  al3;  //  1st  row 

double  a21,  a22,  a23;  //  2nd  row 

double  a31,  a32,  a33;  //  3rd  row 

}; 

To  qualify  as  a  rotation,  the  3  matrix  A  must  satisfy  the  2  conditions:  A^  =  A  1  and  det  A  =  1. 

eIn  general,  rotations  do  not  commute,  i.e.  ffyify  7^  B2R1,  so  the  order  is  significant  and  goes  from  right 

to  left. 

1 A  (rotation)  sequence  is  defined  in  the  Rotation  class  as  follows: 

struct  sequence  { 

double  first;  //  1st  rotation  (rad)  to  apply  to  body  axis 
double  second;  //  2nd  rotation  (rad)  to  apply  to  body  axis 
double  third;  //  3rd  rotation  (rad)  to  apply  to  body  axis 
}; 

The  order  these  are  applied  is  always  left  to  right:  first,  second,  third.  How  they  get  applied  is  specified 
by  using  one  of  the  following,  which  is  applied  left  to  right:  ZYX,  XYZ,  XZY,  YZX,  YXZ,  ZYX,  ZYZ,  ZXZ, 
YZY,  YXY,  XYX,  XZX.  For  example,  the  order  XYZ  would  apply  first  to  rotation  about  the  axaxis,  second 

to  rotation  about  the  y-axis,  and  third  to  rotation  about  the  t-axis. 
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Intentionally  left  blank. 
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Appendix  C.  Quaternion  Algebra  and  Vector  Rotations 
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C-1.  Quaternion  Multiplication 


Starting  with  the  multiplication  rule 

i2  =  j2  =  k2  =  !jk  =  -1,  (C-1) 

it  then  follows  that 

ij  =  -ji  =  k,  jk  =  -kj  =  i,  and  ki  =  -ik  =  j.  (C-2) 

These  rules  are  then  sufficient  to  establish  any  other  multiplication.  Thus,  let 

ql  =  Wi  +  vl=wl  +  rxx  +  jy,  +  kz\ 
q-2  =  w2  +  v2  =  w2  +  ix2  +  jy2  +  kz2 

be  2  quaternions.  Unlike  vectors,  where  there  are  2  different  products — the  scalar 
product  and  the  vector  product — in  the  case  of  quaternions  there  is  only  one  product, 
as  follows: 


qxq2  =  {wi  +  ixi  +  jyi  +  k  zi)(w2  +  ix2  +  jy2  +  k^2) 

=  (wiw2  -  xix2  -  yiy2  -  zi_z2) 

+  i  (wix2  +  w2x i  +  yxz2  -  ziy2) 

+  j  (wiy2  +  w2yi  +  zxx2  -  x1z2) 

+  k  (wiZ2  +  w2zl  +  aqy2  -  y^x2) 

=  w\w2  -vi-v2  +  wiv2  +  w2v\  +  Vi  x  v2.  (C-3) 

Thus,  if  we  represent  a  quaternion  as  an  ordered  pair,  q  =  (iv,  v),  of  a  scalar  and  a 
vector,  then 

(wi,  V1)(w2,  v2)  =  (lViW2  -  V1  ■  V2,  WiV2  +  W-2Vi  +  Vi  x  v2).  (C-4) 

The  scalar  part  of  the  product  is 

WiW2  -  v1-  v2 

and  the  vector  part  is 

WXV2  +  W2v  1  +  Vi  X  v2. 
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C-2.  Quaternion  Division 


Let  q  =  w  +  v  he  a  unit  quaternion,  in  the  sense  that  vr  f  ||n||2  =  1.  Then 
q  1  =  w  —  v  is  the  inverse ,  since 

qq~l  =  (w,  v)(w,  — v ) 

=  ( w 2  —  v  ■  (—v),  w{—v)  +  wr  +  v  x  (— v)) 

=  ( w 2  +  ||v||2,  0) 

=  1.  (C-5) 


Thus,  the  inverse  of  a  unit  quaternion  is  the  quaternion  with  a  negative  vector  part. 
In  effect,  this  serves  to  define  quaternion  division.* 


C-3.  Rotation  of  a  Vector 

Let  a  be  an  arbitrary  vector,  let  u  be  a  unit  vector  along  the  axis  of  rotation,  and  let  9 
be  the  angle  of  rotation.  The  (unit)  quaternion  that  represents  the  rotation  is  given  by 


Q  = 


e  e\ 

cos -,u  sm-  I 


(C-6) 


Then  the  rotated  vector,  a'  is  given  by 


a  =  qaq  1 

=  (w,v)(0,a)(u;,-w) 

=  (w,  v)(a  ■  v,  wa  —  a  x  v) 

=  ( wa  ■  v  —  v  ■  {wa  —  ax  v),w{wa  —  a  x  v)  +  (a  ■  v)v  +  v  x  {wa  —  ax  v )) 

=  (0,  w2a  +  wv  x  a  +  (a  ■  v)v  +  wv  x  a  +  v  x  {v  x  a)) 

=  (0,  w2a  +  v2a  —  v2a  +  (a  ■  +  2wv  x  a  +  v  x  {v  x  a)) 

=  (0, a  +  2wv  x  a  +  2v  x  {v  x  a)),  (C-7) 


where  we  used  the  fact  that  w2  +  v 2  =  1  and  (a  ■  v)v  —v2a  =  v  x  (v  x  a).  Therefore, 


*Quaternions  form  what  is  known  as  a  division  algebra ,  meaning  that  every  non-zero  quaternion 
has  a  multiplicative  inverse.  Vectors  by  themselves  form  an  algebra  but  without  division.  For  an 
interesting  discussion  of  the  relative  merits  of  Hamilton’s  quaternions  and  Gibbs’  vectors,  see 
Chappell  JM,  Iqbal  A,  Hartnett  JG,  Abbott  D.  The  vector  algebra  war:  a  historical  perspective.  Proc 
IEEE.  2016:4:1997-2004. 
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the  rotated  vector  is  given  by 


a  =  a  +  2 wv  x  a  +  2v  x  (v  x  a) 


Using  w  =  cos  0/2  and  it  =  ft  sin  0/2,  we  also  have 


(C-8) 


a  =  a  +  ft  x  a  sin  0  +  ft  x  (ft  x  a)  (1  —  cos  0)  , 


(C-9) 


where  we  made  use  of  the  half-angle  formulas  2  cos (0/2)  sin(0/2)  =  sin0  and 
2  sin2  (0/2)  =  1  —  cos  0. 

Since  this  is  such  a  fundamental  formula,  let  us  derive  it  in  another  way.  For  an 
arbitrary  vector  a,  we  can  always  write 

a  =  a  —  (a  •  u)u  +  (a  •  u)u,  (C-10) 


where  the  third  term  on  the  right  is  the  component  of  a  that  is  parallel  to  u  and  so 
will  remain  unchanged  after  a  rotation  about  u.  The  first  2  terms  form  the  component 
of  a  that  is  perpendicular  to  u  and  will  be  rotated  into 

[a  —  (a  •  u)u]  cos  0  +  u  X  [a  —  (a  •  u)u]  sin  0.  (C-l  1) 


Hence, 

a  =  Ra  =  [a  —  (a  •  u)u]  cos  0  +  u  X  [a  —  (a  •  ■&)■&]  sin  0  +  +(a  • 

=  [a  —  (a  •  cos0  +  u  X  asin0  +  (a  •  ft)!!.  (C-12) 


Now, 

m  X  (a  X  m)  =  a(ft  •  ft)  —  u[u  •  a)  =  a  —  (a  •  ft)ft, 

and  therefore 

a  =  u  X  (a  X  u)  cos  0  +  ft  X  a  sin  0  +  a  —  ft  X  (a  X  ft) 

=  —ft  X  (ft  X  a)  cos  0  +  ft  X  a  sin  0  +  a  +  ft  X  (ft  X  a) 

=  a  +  ft  X  a  sin0  +  ft  X  (ft  X  a)  (1  —  cos0).  (C-13) 
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Appendix  D.  Fundamental  Theorem  of  Rotation  Sequences 
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Fundamental  Theorem  of  Rotation  Sequences:  A  rotation  sequence  about  body 
axes  is  equivalent  to  the  same  rotation  sequence  applied  in  reverse  order  about  fixed 
axes. _ 

The  conventional  way  of  performing  a  rotation  sequence  is  to  account  for  the 
transformation  of  the  body  axes  of  the  object  we  are  rotating.  For  example,  if  we 
wanted  to  first  perform  pitch  about  the  x-axis,  followed  by  yaw  about  the  y-axis,  and 
ending  with  roll  about  the  z-axis,  then  the  rotation,  applied  right  to  left,  is 


R  R\^"  i/Pr)  Ry  (Py)  R-iiPp) : 


(D-l) 


where  j'  =  Ri((j)p)  j,  k;  =  Ri(4>P)  k,  and  k"  =  Ry(4>y)  k;  =  Ry  ((j)y)  Ri((j)p)  k.  But  it 

is  a  fundamental  result  of  rotation  sequences  that  you  get  the  same  result  by  applying 
the  rotation  sequence  in  reverse  order  about  fixed  axes.  That  is, 


(D-2) 


which  is  simpler  and  more  efficient.  This  can  be  proved  with  quaternions  as  follows. 
We  use  the  notation, 

0  0 

q*\<p)  =  cos  -  +  u  sin  -  (D-3) 

for  the  unit  quaternion  that  represents  a  counterclockwise  rotation  of  0  radians  about 
the  unit  vector  u.  Then, 

R\  =  Qe  i(0l)- 


where 


so  that 


D  / i  \  .  V2 

R2  =  Qe'2  Vfo)  =  cos  —  +  e2  sm  — , 


K  =  gei(0i)e2ge11((/)i)> 


(D-4) 

(D-5) 

(D-6) 


02  /  02 
q^m)  =  cos  y  +  e2 sin  — 

02  /  \  —1  \  02 
=  cos  y  +  gg1(0i)e2ggi  (00  sin  — 


=  ggl(0 1)  (  cos  y  +  e2  sin  —  )  q&i  (00 


=  9ei(0l)ge2(02)96l  (00 


(D-7) 
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1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

14 


And 


E>  (A  \  03  ,  -'ll  ■ 

#3  =  9e"(03)  =  COS  —  +  e3  Silly, 


where 


(D-8) 


e's  =  7e'2(02)e3gr,1(02) 

=  7e'2(02)7e1(</>i)e3gg11((/>i)gr,1((/.2) 

=  fei(</,l)7e2(02)gg11(0l)]ge1(0l)e3gg11(0l)[ge1(0l)ge2(02)ge'11((/>l)]'1 

=  7e1(0i)ge2(02)gg11(0i)ge1(0i)e3gr1(01)gg1(01)gr1(02)gr1(01) 

=  7e1(0i)7e2(02)e3gr1((/)2)gri1(01),  (D-9) 

so  that 


Qegife' 


03  .  -//  .  03 
=  COS  y  +  e3  Sin  y 


=  COS-^  +  ge1(0l)7e2(02)e37e21(02)7e11(0l)sin  — 


2ei 


=  9ei  (0l)9e2  (02)  (^COS  y  +  e3  sin  y^  9^  (02)  9^(01) 

=  9e1(0l)9e2  (02)963(03)9^  (02)9g11(0l)- 


(D-10) 


Therefore,  the  total  combined  rotation  is 


R  =  R3R2R1  =  9e"(03)9e^(02)9e1(0l) 

=  [96!  (0l)962  (02)963  (03)9e21(02)C11(0l)]  fex  (0l)9e2  (02)9^  (01  )]9ex  (0l) 

=  963(01)962(02)963(03),  (D-ll) 

as  was  to  be  shown. 


The  program  in  Listing  D-l  is  designed  to  test  this  result. 


Listing  D-l.  order.cpp 

//  order.cpp:  demonstrate  that  rotation  about  fixed  axis  in  reverse  order  is  correct 
//  for  both  rotation  about  distinct  axes  and  for  rotation  about  repeated  axes 

#include  "Rotation. h" 

#include  <iostream> 

#include  <cstdlib> 
using  namespace  std; 

int  main(  int  argc,  char*  argv[]  )  { 

va: : Vector  i(  1.,  0.,  0.  ),  j(  0.,  1.,  0.  ),  k(  0.,  0.,  1.  ),  v(  1.2,  -3.4,  6.7  ); 
va::Vector  il,  jl,  kl,  i2,  j2,  k2,  i3,  j 3 ,  k3; 
va:: Rotation  R,  Rl,  R2,  R3; 

va::0RDER  order  =  va::XYZ;  //  select  rotation  sequence  about  distinct  axes 
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15 

double  ang_l  = 

0. ; 

16 

double  ang_2  = 

0. ; 

17 

double  ang_3  = 

0. ; 

18 

if  (  argc  ==  4 

)  { 

19 

20 

ang_l  =  va: 

rad( 

atof  ( 

argv[ 

1  ]  )  ); 

21 

ang_2  =  va: 

rad( 

atof  ( 

argv[ 

2  ]  )  ) ; 

22 

ang_3  =  va: 

rad( 

atof  ( 

argv[ 

3  ]  )  ); 

23  > 

24 

25  R  =  va: : Rotation (  ang_l,  ang_2,  ang_3,  order  ); 

26 

27  cout  «  "The  constructed  rotation  is  "  «  R  «  endl; 

28  cout  «  "The  rotated  vector  is  "  «  R  *  v  «  endl; 

29  cout  «  "The  following  rotations  should  match  this:"  «  endl; 

30 


31 

Rl  =  va: 

: Rotation!  i 

,  ang_l  ) ;  //  rotation 

about  x-axis 

32 

R2  =  va: 

: Rotation!  j 

,  ang_2  ) ;  //  rotation 

about  y-axis 

33 

R3  =  va: 

: Rotation!  k,  ang_3  );  //  rotation 

about  z-axis 

34 

35 

R  =  Rl  * 

R2  *  R3; 

//  note  the  order  is  the 

reverse:  first  3,  then  2,  then  1 

36 

cout  « 

"Reverse  order  about  fixed  axes:"  « 

endl; 

37 

cout  « 

" Rotation : 

"  «  R  «  endl; 

38 

cout  « 

"Rotated  vector:  "  «  R  *  v  «  endl; 

39 

40  cout  «  endl  «  "Now  the  conventional  way  via  transformed  axes:"  «  endl  «  endl; 

41 

42  //  first  rotation  is  about  i 

43  R1  =  va: :Rotation(  i,  ang_l  );  //  rotation  about  x-axis 

44  il  =  R1  *  i; 

45  jl  =  R1  *  j; 

46  kl  =  R1  *  k; 

47 

48  //  second  rotation  is  about  jl 

49  R2  =  va: :Rotation(  jl,  ang_2  );  //  rotation  about  transformed  y-axis 

50  i2  =  R2  *  il; 

51  j2  =  R2  *  jl; 

52  k2  =  R2  *  kl; 

53 

54  //  third  rotation  is  about  k2 

55  R3  =  va :: Rotation!  k2,  ang_3  );  //  rotation  about  doubly  transformed  z-axis 

56  i3  =  R3  *  i2; 

57  j  3  =  R3  *  j  2 ; 

58  k3  =  R3  *  k2; 

59 

60  R  =  R3  *  R2  *  Rl;  //  note  the  order  is  the  original:  first  1,  then  2,  then  3 

61 

62  cout  «  "Original  order  about  transformed  axes:"  «  endl; 

63  cout  «  "Rotation:  "  «  R  «  endl; 

64  cout  «  "Rotated  vector:  "  «  R  *  v  «  endl; 

65 

66  cout  «  endl  «  "This  also  works  for  repeated  axes."  «  endl 

67  «  "Using  the  same  rotation  angles,  let's  do  the  whole  thing  over  again."  «  endl  «  endl; 

68  order  =  va::XYX;  //  select  rotation  sequence  about  repeated  axes 

69 

70  R  =  va: : Rotation!  ang_l,  ang_2,  ang_3,  order  ); 

71 

72  cout  «  "The  constructed  rotation  is  "  «  R  «  endl; 

73  cout  «  "The  rotated  vector  is  "  «  R  *  v  «  endl; 

74  cout  «  "The  following  rotations  should  match  this:"  «  endl; 

75 


76 

Rl  =  va: : Rotation!  i, 

ang_l  ) ; 

// 

rotation 

about 

x-axis 

77 

R2  =  va: : Rotation!  j , 

ang_2  ) ; 

// 

rotation 

about 

y-axis 

78 

R3  =  va: : Rotation!  i, 

ang_3  ) ; 

// 

rotation 

about 

x-axis 

79 

80  R  =  Rl  *  R2  *  R3;  //  note  the  order  is  the  reverse:  first  3,  then  2,  then  1 

81  cout  «  "Reverse  order  about  fixed  axes:"  «  endl; 

82  cout  «  "Rotation:  "  «  R  «  endl; 

83  cout  «  "Rotated  vector:  "  «  R  *  v  «  endl; 

84 

85  cout  «  endl  «  "Now  the  conventional  way  via  transformed  axes:"  «  endl  «  endl; 

86 

87  //  first  rotation  is  about  i 

88  Rl  =  va: :Rotation(  i,  ang_l  );  //  rotation  about  x-axis 

89  il  =  Rl  *  i; 

90  jl  =  Rl  *  j; 

91  kl  =  Rl  *  k; 

92 

93  //  second  rotation  is  about  jl 

94  R2  =  va: :Rotation(  jl,  ang_2  );  //  rotation  about  transformed  y-axis 

95  i2  =  R2  *  il; 

96  j2  =  R2  *  jl; 

97  k2  =  R2  *  kl; 

98 

99  //  third  rotation  is  about  i2 
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100 

101 

102 

103 

104 

105 

106 

107 

108 

109 

110 

111 

112 

1 

1 

2 

3 

4 

5 

6 
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8 
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10 
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17 

18 

19 

20 

21 

22 

23 

24 

25 

26 

27 

28 


R3  =  va: :Rotation(  i2,  ang_3  );  //  rotation  about  doubly  transformed  x-axis 

i3  =  R3  *  i2; 
j3  =  R3  *  j 2 ; 
k3  =  R3  *  k2; 

R  =  R3  *  R2  *  Rl;  //  note  the  order  is  the  original:  first  1,  then  2,  then  3 

cout  «  "Original  order  about  transformed  axes:"  «  endl; 
cout  «  "Rotation:  "  «  R  «  endl; 

cout  «  "Rotated  vector:  "  «  R  *  v  «  endl; 

return  EXIT_ SUCCESS; 

> 


The  command 

./order  35.  -15.  60. 


will  give  the  following  results: 


The  constructed  rotation  is  -0.533171  -0.0686517  -0.843218  73.8825 

The  rotated  vector  is  -0.530512  1.81621  7.36953 

The  following  rotations  should  match  this: 

Reverse  order  about  fixed  axes: 

Rotation:  -0.533171  -0.0686517  -0.843218  73.8825 

Rotated  vector:  -0.530512  1.81621  7.36953 

Now  the  conventional  way  via  transformed  axes: 

Original  order  about  transformed  axes: 

Rotation:  -0.533171  -0.0686517  -0.843218  73.8825 

Rotated  vector:  -0.530512  1.81621  7.36953 

This  also  works  for  repeated  axes. 

Using  the  same  rotation  angles,  we  do  the  whole  thing  over  again. 

The  constructed  rotation  is  -0.984429  0.171618  0.0380469  95.8951 

The  rotated  vector  is  2.78824  6.66967  2.37301 

The  following  rotations  should  match  this: 

Reverse  order  about  fixed  axes: 

Rotation:  -0.984429  0.171618  0.0380469  95.8951 

Rotated  vector:  2.78824  6.66967  2.37301 

Now  the  conventional  way  via  transformed  axes: 

Original  order  about  transformed  axes: 

Rotation:  -0.984429  0.171618  0.0380469  95.8951 

Rotated  vector:  2.78824  6.66967  2.37301 
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Intentionally  left  blank. 
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Appendix  E.  Factoring  a  Rotation  into  a  Rotation  Sequence 
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E-1.  Distinct  Principal  Axis  Factorization 


The  most  common  rotation  sequence  is  probably  the  aerospace  sequence,  which 
consists  of  yaw  about  the  body  x-axis,  pitch  about  the  body  y-axis,  and  roll  about 
the  body  x-axis — in  that  order.  However,  there  are  a  total  of  6  such  distinct  principal 
axis  rotation  sequences  and  we  will  factor  each  one.1 

Let  us  begin  with  the  z-y-x  (aerospace)  rotation  sequence,  consisting  of  yaw  about 
the  x-axis,  followed  by  pitch  about  the  y-axis  and  ending  with  roll  about  the  x-axis. 

Let  the  given  rotation  be  represented  by  the  quaternion 


p  =  po  +  ipi  +  j P2  +  k p3.  (E-1) 

In  the  notation  of  Kuipers1  (see  pp.  194-196),  we  want  to  factor  this  as  a3b2c1,  so 
we  write 

P  =  (a0  +  ka3)(&0  +  j&2)(c0  +  ici).  (E-2) 

Let  q  represent  the  first  2  factors: 

q  =  (o0  +  ka3)(f>0  +  j 62)  =  aob0  -  ia3&2  +  ja0b2  +  ka3fe0-  (E-3) 


Then 

q  =  p^c1)-1  =  (p0  +  ip  1  +  jy2  +  kp3)(c0  -  ici) 

=  ( P0C0  +  P1C1)  +  i(piCo  -  poci)  +  j(y2c0  -  p3c3)  +  k(y3c0  +  p2ci),  (E-4) 

from  which  we  identify 


<?o  =PoCo  +  PiCi,  qi  =  P1C0  ~  P0C1,  q2  =  P2C0  ~  P3C1,  g3  =  p3c0  +  p2Ci.  (E-5) 

The  constraint  equation  for  this  to  be  a  tracking  rotation  sequence  follows  from 
Eq.  E-3: 


<Ml  +  <?2<?3 


qo  qi 


q  1 

qa 


( a0b0)(-a3b2 )  +  (a0b2)(a3b0)  =  0.  (E-6) 


'Kuipers  JB.  Quaternions  and  rotation  sequences:  a  primer  with  applications  to  orbits,  aerosp 
ace,  and  virtual  reality.  Princeton  (NJ):  Princeton  University  Press;  2002. 
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Now,  from  Eq.  E-5, 


and 


% 

q-2 

Qi 

Qs 


PlCo  -  PoCl 
PsC0  +  p2C 1 


Po 

Pi 

CO 

- 

Pi 

-P3_ 

Cl 

Pi 

- Po 

c0 

P3 

P2_ 

Cl 

so  the  constraint  equation,  Eq.  E-6,  may  be  written  as 


Co  C 1 


Po  P  2 
Pi  Pi 


Pi 

P3 


-Po 

P2 


r  1 

popi  +  P2P3  -p'i + p'i 

Co 

Co  Cl 

9  9 

Pi  -  Pi  -P0P1  -  P2P3 

Cl_ 

=  0. 

J  [  p'i -pi  -popi  - P2P3J  LCiJ 

Define  the  quantities 

A  =  P0P1  +  P2P3,  B  =  -po  +  pi,  D  =  p{  -  p\. 

Then  the  constraint  equation  becomes 


Co  Cl 


A  B 
D  -A 


—  A(cq  —  cl)  +  (f?  +  D)c0ci  —  0. 


Finally,  this  may  be  written  as 

2A  2c0ci 


O  'TO  •  'TO 

Z  cos  —  sin  — 

2  2 


B  +  D  %-cf 


cos 


sin  03 

.  2  ^3  COS  03 
sm  — 


—  tan  03, 


where  we  used 


Co  =  cos  A-  and  ci  =  sin  A-. 


Therefore,  the  final  rotation  (roll  angle  in  this  case)  is 

-2  A 


=  tan 


-1 


B  +  DJ  ’ 

and  this  quantity  is  known  since  A,  B,  and  D  are  known  from  Eq.  E-10. 
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since 


(E-15) 


3  ,r  ri  ,  i  •  ri 

a  =  a>o  +  ka3  =  cos  —  +  k  sm  — , 


it  follows  that 


01  «3  a3^0  Q3 

tan  —  =  —  =  — —  =  — , 
2  cio  do^o  % 


(E-16) 


from  Eq.  E-3.  Therefore,  the  first  rotation  (yaw  angle  in  this  case)  is  given  by 


Similarly, 


=  2  tan  1  [  —  )  . 

Mo. 


02  b2  aob2  q2 
tan  —  =  —  = 


(E-17) 


(E-18) 


2  b0  a0b0  q0  ’ 

again  using  Eq.  E-3,  and  therefore  the  second  rotation  (pitch  angle  in  this  case)  is 
given  by 

=  2  tan-1  ( —  )  .  (E-19) 


Qo 

In  summary,  the  prescription  for  factoring  an  arbitrary  rotation  into  yaw  (about  the 
z-axis),  pitch  (about  the  y-axis),  and  roll  (about  the  rc-axis)  (in  that  order)  is  given  in 
Table  E-l. 


Table  E-l.  Factorization  into  z-y-x  (aerospace)  rotation  sequence,  consisting  of  yaw  about  the 
2-axis,  pitch  about  the  y-axis,  and  roll  about  the  tc-axis 


A  =  P0P1+P2P3, 

B  = 

P\  -  P2o,  D  =  pj-  p\ 

1  ,  (  -2A  3 

|  [third  rotation,  roll  about  x-axis] 

03  \B  +  Dj 

03 

C0  =  COSy, 

■  03 

Ci  =  sin  — 

2 

qo  =  P0C0  +  Pi  Cl,  qi  =  PiC0  - 

P0C1, 

=P2C0  -P3C1,  q3=P3Co+P2Cl 

0i  =  2  tan-1  (  —  j 

[first  rotation,  yaw  about  ^-axis] 

02  =  2  tan-1 

[second  rotation,  pitch  about  y-axis] 

The  calculations  for  the  other  5  sequential  orders  are  entirely  similar  and  we  simply 
summarize  the  results  in  Tables  E-2  through  E-6. 
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Table  E-2.  Factorization  into  x-y-z  (FATEPEN)  rotation  sequence,  consisting  of  pitch  about 
the  .r-axis,  yaw  about  the  y-axis,  and  roll  about  the  2>axis 


to 

1 

o 

CO 

B  =  p\-v  I,  D=pl-pl 

,  .  (  -2 A  \ 

[third  rotation,  roll  about  z-axis] 

^  \B  +  d) 

03  .  03 

c0  =  cos  — ,  c3  =  sm  — 
u  2  2 

Qo  =  PoCo  +  P3C3,  qi  =  pic0  - 

P2C3,  q2  =  P2C0  +  P1C3,  q3  =  p3c0  -  p0c3 

0i  =  2  tan-1  ^ 

[first  rotation,  pitch  about  re-axis] 

02  =  2  tan-1  (  —  ) 

\qoJ 

[second  rotation,  yaw  about  y-axis] 

Table  E-3.  Factorization  into  y-x-z  rotation  sequence 

A  =  P1P2  +  P0P3, 

b  =  pI  +  pI  D  =  -pl-pl 

2  ,  (  -2 A 

A f  0  n  J-  1 

I  th  1  rr\fofmn  oKr\nf  -y_ovicl 

u/x  —  lcljli  1  „  „  1  i_i  11 1  va  tutauuii  auuut  0  ctzvxo 

\B  +  Dj 

03  .  03 

c0  =  cos  — ,  c3  =  sm  — 
u  2  2 

Qo  =  PoCo  +  P3C3,  q\  =  P1C0  ~ 

P2C3 ,  q2  =  P2C0  +  p  1  cs )  q3  =  P3C0  -  P0C3 

0i  =  2  tan-1  (  — 
\qo. 

^  [first  rotation  about  y-axis] 

0  2  =  2  tan-1  ^ 

[second  rotation  about  re-axis] 
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Table  E-4.  Factorization  into  z-x-y  rotation  sequence 


a  =  pm  -  P0P2,  B  =  pl-pl,  d=pI~p22 


=  tan 


-1 


-2  A 

B  +  D 


[third  rotation  about  y-axis] 


Co  =  COS - ,  Co  =  sm  — 

u  2  2 


qo  =  P0C0  +P2C2,  qi  =  P1C0  +P3C2,  q2  =  P2C0  ~  P0C2,  <?3  =  P3C0  -  P\C2 

0i  =  2  tan-1  (  —  )  [first  rotation  about  2-axis] 

\qoJ 


,  =  2  tan  1  (  —  I  [second  rotation  about  x-axis] 

' _ VW _ 

Table  E-5.  Factorization  into  x-z-y  rotation  sequence 


A  =  P1P3  +  PoP2,  B  —  —pi  —  pj,  D  =pl+pl 


=  tan 


-1 


-2  A 

B  +  D 


[third  rotation  about  y-axis] 


C0  =  COSy,  C2  =  Silly 


qo  =PoC0  +P2C2,  qi  =  P1C0  +P3C2,  q2  =  P2C0  -  P0C2,  <?3  =  P3C0  -  P+2 

0i  =  2  tan-1  (  —  )  [first  rotation  about  x-axis] 

\qoJ 


I  <73 

=  2  tan-1  (  —  )  [second  rotation  about  ^-axis] 

Ao, 


Table  E-6.  Factorization  into  y-z-x  rotation  sequence 


A  =  P2P3  -  P0P1,  B  =  pl  +  pl,  D  =  -pj  -  pj 


=  tan 


-1 


-2  A 


B  +  D 


[third  rotation  about  rr-axis] 


C0  =  COSy,  Ci  =  Silly 

qo  =  P0C0  +  P1C1,  qi  =  P1C0  -P0C1,  <?2  =  P2C0  ~  P3C1,  q3  =  P3C0  +  P2+ 

0i  =  2  tan-1  (  —  )  [first  rotation  about  y-axis] 

\qoJ 


=  2  tan  1  (  —  I  [second  rotation  about  2-axis] 

Ao, 


Approved  for  public  release;  distribution  is  unlimited. 


60 


E-2.  Repeated  Principal  Axis  Factorization 


We  define  a  repeated  principal  axis  sequence  as  first  a  rotation  about  one  of  the 
principal  body  axes,  then  a  second  rotation  about  another  body  axis,  and  finally  a 
third  rotation  about  the  first  body  axes.  There  are  a  total  of  6  such  rotation  sequences 
and  we  will  factor  each  one.1 

We  begin  with  the  z-y-z  rotation  sequence,  consisting  of  first  about  the  z-axis,  second 
about  the  y- axis  and  third  about  the  z-axis 

Let  the  given  rotation  be  represented  by  the  quaternion 


P  =  Po  +  ipi  +  JP2  +  k p3.  (E-20) 

In  the  notation  of  Kuipers1,  we  want  to  factor  this  as  a362c3,  so  we  write 

P  =  (ao  +  ka3)(&0  +  j&2)(c0  +  kc3).  (E-21) 

Let  q  represent  the  first  2  factors: 

q  =  (do  +  ka3)(&0  +  J&2)  =  a0b0  -  i a3b-2  +  j a0b2  +  k a3b0.  (E-22) 


Then 


q  =  p^c1)  1  =  (p0  +  ip  1  +JP2  +  kp3)(c0  -  kc3) 

=  ( P0C0  +  P3C3)  +  i(PiC0  -  P2C3 )  +  j(p2c0  +  Pic3)  +  k(p3c0  -  p0c3),  (E-23) 


from  which  we  identify 


<?o  =  P0C0+P3C3,  <1\  =  P1C0  -P2C3,  Q2  =  P2C0+P1C31  <?3  =  P3C0-P0C3.  (E-24) 

The  constraint  equation  for  this  to  be  a  tracking  rotation  sequence  follows  from 
Eq.  E-22: 


+  <M3 


qo  q2 


q\ 

93 


(ao^o)(— ^3^2)  +  (do^2)(d3^o)  —  0.  (E-25) 


'See  Kuipers,  pp.  200-201,  for  the  technique,  but  note  that  there  is  a  typo  in  Eq.  8.31,  which 
leads  to  an  error  in  Eq.  8.32.  This  has  been  corrected  here. 
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Now,  from  Eq.  E-24, 


% 

P0C0  +  P3C3 

Po 

P3 

Co 

q2_ 

P2C0  +  P1C3 

P2 

Pl_ 

_c3_ 

1 

r 

r 

1 

r  n 

and 


so  that  the  constraint  equation,  Eq.  E-25,  may  be  written  as 


Qi 

PlCo 

-P2C3 

Pi 

-P2 

Co 

q3 

P3C0 

-P0C3 

: P 3 

-Po_ 

C3 

Co  C3 


Co  C3 


Po  P2 
P3  Pi 


Pi 

P3 


P'2 

Po 


P0P1  +  P2P3 
PlP3  +  PlP3 


-POP2  -  POP2 
-P2P3  -  P0P1 


Co 

_C3_ 

=  0. 


Define  the  quantities 

A  =  P0P1  +  P2P3,  B  =  -2p0p2,  D  =  2pip3. 

Then  the  constraint  equation  becomes 


Co  C3 


A  B 
D  -A 


—  A(cq  —  cl)  +  (B  +  D)c0c3  —  0. 


Finally,  this  may  be  written  as 


0  r  O  •  'TO 

2A  _  2c0c3  _  2  cos  y  sm  — 


sin  cj)  3 


B  +  D  cl  —  Co  2  03  .  2  03  COS  03 

u  J  cos2 - sm  — 


—  tan  03, 


where  we  used 


'TO  ,  . 

Cn  =  cos  —  and  c3  =  sm  — . 
u  2  2 


Therefore,  the  final  rotation  is 


=  tan 


-1 


-2A 


B  +  DJ  ’ 

and  this  quantity  is  known  since  A,  B,  and  D  are  known  from  Eq.  E-29. 
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(E-26) 

(E-27) 

(E-28) 

(E-29) 

(E-30) 

(E-31) 

(E-32) 

(E-33) 

Furthermore, 
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since 


(E-34) 


3  ,r  ri  ,  i  •  ri 

a  =  a>o  +  ka3  =  cos  —  +  k  sm  — , 


it  follows  that 


01  «3  a3^0  q3 

tan  —  =  —  =  — —  =  — . 
2  ag  «(wo  % 


where  we  used  Eq.  E-22.  Therefore,  the  first  rotation  is  given  by 


Similarly, 


=  2  tan-1  (  — 
<?o 


02  b2  aob2  q2 

tan  —  —  —  — - =  — , 

2  b0  a0b0  q0  ' 


(E-35) 


(E-36) 


(E-37) 


again  using  Eq.  E-22,  and  therefore  the  second  rotation  (pitch  angle  in  this  case)  is 
given  by 

,  f  dn  \ 

(E-38) 


=  2  tan  1  f  —  )  . 


Qo 

In  summary,  the  prescription  for  factoring  an  arbitrary  rotation  into  an  Euler  sequence 
of  first  a  rotation  about  the  z-axis,  followed  by  a  rotation  about  the  body  y-axis,  and 
finally  ending  with  a  rotation  about  the  body  £-axis,  is  given  in  Table  E-7. 


Table  E-7.  Factorization  into  z-y-x  rotation  sequence 


A  =  P0P1  +  P2P3, 

B  =  -2p0p2,  D  =  2pip3 

2  1  /  -2A  N 

/T)r,  E  O  TV  -*-1 

|  [third  rotation  about  z-axis J 

C/zQ  -  belli  1 

Kb  +  d, 

Co  =  cos 

03  .  03 

- ,  C3  =  sin  — 

2  2 

qo  =  P0C0  +  P3C3,  qi  =  P1C0  -  P2C3,  q2  =  P2C0  +  PiC3,  q3  =  p3c0  -  P0C3 

0i  =  2  tan  1  (  —  ) 

\qoJ 

[first  rotation  about  z-axis] 

09  =  2  tan-1  f  —  ) 

\qo  J 

[second  rotation  about  y-axis] 

The  calculations  for  the  other  5  sequential  orders  are  entirely  similar  and  we  simply 
summarize  the  results  in  Tables  E-8  through  E-12. 
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Table  E-8.  Factorization  into  z-x-z  rotation  sequence 


A  =  P0P2  ~  P1P3,  B  =  2p0p1,  D  =  2p-2p3 


=  tan 


-1 


-2  A 
B  +  D 


[third  rotation  about  2-axis] 


C0  =  COSy,  C3=  Silly 


Qo  =  P0C0  +  P3C3,  qi  =  P1C0  ~  P2C3,  q2  =  P2C0  +  PiC3,  q3  =  p3c0  -  p0c3 


=  2  tan  1  (  — 
SB 


[first  rotation  about  2-axis] 


=  2  tan  1  (  —  I  [second  rotation  about  tr-axis] 

Ao, 


Table  E-9.  Factorization  into  y-z-y  rotation  sequence 


A  =  PoPi~P2P3i  B=pQp3+plp2 


0  3  =  tan 


-1 


-A 

If 


[third  rotation  about  y-axis] 


C0  =  COSy,  C-2=  Silly 


qo  =  PoC0  +P2C2,  qi  =  P1C0  +  P3C2,  q-2  =  P2C0  -  PoC2,  q-3=p3c0-PiC2 

if  2 


=  2  tan  1  —  [first  rotation  about  y-axis] 

Ao, 


1  Qs 

=  2  tan-1  (  —  )  [second  rotation  about  ^-axis] 

Ao, 


Table  E-10.  Factorization  into  y-x-y  rotation  sequence 

A  =  P0P3  +  P1P2,  B  =  —2p0pi,  D  =  2  p2p3 


=  tan 


-1 


-2  A 


B  +  D 


[third  rotation  about  y-axis] 


03  ■  03 

Cn  =  COS - ,  C2  =  Sill - 

u  2  2 

qo  =Poc0  +  P2C2,  qi  =  Pico  +  p3c2,  q2  =  P2C0  -  P0C2,  q3  =  p3c0-piC2 

0i  =  2  tan-1  (  —  )  [first  rotation  about  y-axis] 
_ _ \AoJ _ 

0 2  =  2  tan-1  (  —  )  [second  rotation  about  x-axis] 

\%J 
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Table  E-ll.  Factorization  into  x-y-x  rotation  sequence 


A  =  PoP3  -  P1P2,  B  =  p0p2  +  PlP3 


[third  rotation  about  x-axis] 


03  .  03 

C0  =  COSy,  Cl  =  Silly 


qo  =  PoCo  +  PiCi,  qi  =  PiCo  ~  PoCi,  q2  =  p2c0  -  p3Ci,  q3  =  P3C0  +  P2C1 


0i  =  2  tan-1  (  —  )  [first  rotation  about  x-axis] 

\qoJ 


02  =  2  tan  1  (  —  )  [second  rotation  about  y-axis] 

\qoj 


Table  E-12.  Factorization  into  x-z-x  rotation  sequence 


A  =  P0P2  +  P1P3,  B  =  P0P3  +  P1P2 


[third  rotation  about  x-axis] 


03  =  tan 


-1 


-A 


B 


=  tan 


-1 


-A 

0B~ 


Qo  =PoC0  +P1C1,  qi=PiC0~PoCi,  q2  =  P2C0  -  P3C1,  q3  =  P3C0  +  P2C1 


0i  =  2  tan  1  (  —  )  [first  rotation  about  x-axis] 

\qoj 


02  =  2  tan  1  (  —  )  [second  rotation  about  x-axis] 

\qoJ 


The  program  in  Listing  E-l  is  designed  to  test  these  formulas. 


Listing  E-l.  factor.cpp 

//  factor.cpp:  test  program  for  the  rotation  factorization 

#include  "Rotation. h" 

#include  <iostream> 

#include  <cstdlib> 

int  main(  int  argc,  char*  argv[]  )  { 

va:: Rotation  R; 
rng : : Random  rng; 

va:: ORDER  order  =  va::0RDER(  rng . uniformDiscrete(  0,  11  )  ); 
double  ang_l,  ang_2,  ang_3; 
if  (  argc  ==  4  )  { 


ang_l  =  va::rad(  atof(  argv[  1  ]  )  ) 

ang_2  =  va::rad(  atof(  argv[  2  ]  )  ) 

ang_3  =  va::rad(  atof(  argv[  3  ]  )  ) 

R  =  va :: Rotation (  ang_l,  ang_2,  ang_3,  order  ); 

std::cout  «  "order  =  11  «  order  «  std::endl; 
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25 

26 
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else  if  (  argc  ==  1  )  { 

R  =  va :: Rotation (  rng  ); 

} 

else  { 

std::cerr  «  argv[  0  ]  «  "  usage:  Enter  angles  1,  2,  and  3  (deg)  on  commandline"  «  std::endl; 
exit(  EXIT- FAILURE  ); 

} 

std::cout  «  "The  rotation  is  "  «  R  «  std::endl  «  std::endl;;  //  output  the  rotation 
std::cout  «  "The  following  rotations  should  match  this"  «  std::endl; 

for  (  int  order  =  0;  order  <  12;  order++  )  { 

va:: sequence  s  =  va: : factor (  R,  va::0RDER(  order  )  );  //  factor  the  rotation 

std::cout  «  "1st  rotation  =  "  «  va::deg(  s. first  )  «  "\torder  =  "  «  order  «  std::endl;  //  output  the 

factorization 

std::cout  «  "2nd  rotation  =  "  «  va::deg(  s. second  )  «  std::endl; 

std::cout  «  "3rd  rotation  =  "  «  va::deg(  s. third  )  «  std::endl; 

R  =  va :: Rotation (  s,  va::0RDER(  order  )  );  //  generate  the  rotation  with  this  sequence 

std::cout  «  R  «  std::endl;  //  output  the  rotation  so  that  it  can  be  compared 

> 

return  EXIT-SUCCESS; 


The  command 

./factor 


will  generate  a  random  rotation,  so  each  run  will  be  different.  But  the  factored 
rotation  must  match  the  randomly  generated  rotation,  as  shown  here: 


The  rotation  is  0.18777  0.958993  -0.212307  86.6526 

The  following  rotations  should  match  this 

1st  rotation  =  -24.8355  order  =  0 

2nd  rotation  =  84.2077 

3rd  rotation  =  -2.42093 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  75.1077  order  =  1 

2nd  rotation  =  66.8997 

3rd  rotation  =  -76.5002 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  83.7441  order  =  2 

2nd  rotation  =  22.2818 

3rd  rotation  =  -2.62561 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  -22.4269  order  =  3 

2nd  rotation  =  -0.244254 

3rd  rotation  =  84.2128 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  -0.264239  order  =  4 

2nd  rotation  =  -22.4267 

3rd  rotation  =  84.3136 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  84.7402  order  =  5 

2nd  rotation  =  -2.42944 

3rd  rotation  =  22.303 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  -22.4022  order  =  6 

2nd  rotation  =  84.2129 

3rd  rotation  =  -0.245506 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  -112.402  order  =  7 

2nd  rotation  =  -84.2129 

3rd  rotation  =  89.7545 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  0.640217  order  =  8 

2nd  rotation  =  -22.4282 

3rd  rotation  =  83.621 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  90.6402  order  =  9 

2nd  rotation  =  22.4282 

3rd  rotation  =  -6.37896 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  -2.4397  order  =  10 

2nd  rotation  =  84.745 
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3rd  rotation  =  22.5265 

0.18777  0.958993  -0.212307  86.6526 

1st  rotation  =  87.5603  order  =  11 

2nd  rotation  =  -84.745 

3rd  rotation  =  -67.4735 

0.18777  0.958993  -0.212307  86.6526 


We  can  also  input  an  explicit  rotation: 

./factor  -13.  67.  -23. 


This  gives  the  following  results: 


order  =  1 

The  rotation  is  -0.33597  0.863186  -0.376875  73.8475 

The  following  rotations  should  match  this 

1st  rotation  =  -57.8081  order  =  0 

2nd  rotation  =  47.5373 

3rd  rotation  =  -55.6718 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  -13  order  =  1 

2nd  rotation  =  67 

3rd  rotation  =  -23 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  67.5302  order  =  2 

2nd  rotation  =  -5.04254 

3rd  rotation  =  -34.9977 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  -10.5973  order  =  3 

2nd  rotation  =  -33.8845 

3rd  rotation  =  62.7029 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  -34.3421  order  =  4 

2nd  rotation  =  -8.78174 

3rd  rotation  =  68.6579 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  64.0087  order  =  5 

2nd  rotation  =  -34.8426 

3rd  rotation  =  -6.14787 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  5.45441  order  =  6 

2nd  rotation  =  67.6219 

3rd  rotation  =  -37.0797 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  -84.5456  order  =  7 

2nd  rotation  =  -67.6219 

3rd  rotation  =  52.9203 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  74.6856  order  =  8 

2nd  rotation  =  -35.3132 

3rd  rotation  =  -8.7461 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  -15.3144  order  =  9 

2nd  rotation  =  -35.3132 

3rd  rotation  =  81.2539 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  -37.756  order  =  10 

2nd  rotation  =  68.9201 

3rd  rotation  =  9.4171 

-0.33597  0.863186  -0.376875  73.8475 

1st  rotation  =  52.244  order  =  11 

2nd  rotation  =  -68.9201 

3rd  rotation  =  -80.5829 

-0.33597  0.863186  -0.376875  73.8475 


The  order  variable  is  arbitrary  so  it  is  randomized  in  the  program.  It  is  output  so  that 
we  can  check  that  it  found  the  same  rotation  sequence  for  that  particular  order  (in 
this  case,  order  =1). 
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Intentionally  left  blank. 
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Appendix  F.  Conversion  between  Quaternion  and  Rotation  Matrix 
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F-1.  Quaternion  to  Rotation  Matrix 


For  rotations  about  a  principal  axis,  the  correspondence  is  as  follows: 


e  e 

Rotation  about  the  x-axis:  cos  — b  i  sin  - 

2  2 


9  9 

Rotation  about  the  y- axis:  cos  -  +  j  sin  - 


„  .  ,  .  9  f  .  9 

Rotation  about  the  z-axis:  cos  — b  k  sin  - 

2  2 


In  general,  if  the  quaternion  is  given  by 

q  =  go  +  qii  +  q-2i  +  g3k, 

then  the  corresponding  rotation  matrix  is 


'1 

0 

o' 

0 

cos  9 

- 

sin 

9 

(F-1) 

_0 

sin  9 

cos 

9_ 

” 

cos  9 

0 

sin 

9 

0 

1 

0 

(F-2) 

— 

sin  9 

0 

cos 

9_ 

cos  9 

-  sin  9 

o' 

sin  9 

cos  9 

0 

(F-3) 

0 

0 

1 

A  = 


2q%-l  +  2 q\  2 qxq2  -  2 g0g3  2 gig3  +  2 q0q2 
2gi?2  +  2q0q3  2q g  —  1  +  2  q^  2g2g3  —  2g0gi 
2gig3  -  2<M2  2g2g3  +  2g0gi  2q%-l  +  2qj 


F-2.  Rotation  Matrix  to  Quaternion 


Oil 

O12 

Ol3 

021 

O22 

®23 

031 

032 

«33 

(F-4) 


(F-5) 


Let  the  rotation  be  given  by  the  matrix 

d\ 

A  =  0,9 1  022  a23  •  (F-6) 

I32  ®33_ 

Then  a  vector  along  the  axis  of  rotation  is 

v  —  («32  —  a23)i  +  (013  —  031  )j  +  (a2i  —  Oi2)k.  (F-7) 
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If  this  vector  turns  out  to  be  zero,  then  A  is  the  identity  matrix.  Otherwise,  a  unit 
vector  along  the  axis  of  rotation  is 


u  = 


(«32  —  023)l  +  (013  —  ®3l)j  +  (®21  ~  Oi2)k 
\J (&32  —  023)2  +  (a13  —  03l)2  +  (a21  ~  Ol2)2 


The  rotation  angle  is 


6  =  cos 


-1 


on  +  ci  22  +  033  —  1 


(F-8) 


(F-9) 


And  the  corresponding  quaternion  is 


e  .  .  e 

q  =  cos  — \- u  sm  - . 
1  2  2 


(F-10) 


F-3.  Conversion  between  Rotation,  Rotation  Matrix,  and  Quaternion 


The  program  in  Listing  F-l  will  convert  between  the  3  different  representations. 


Listing  F-l.  convert.cpp 

//  convert.cpp:  convert  between  Rotation,  rotation  matix,  and  quaternion 

#include  "Rotation . h" 

#include  <iostream> 

#include  <cstdlib> 

#include  <iomanip> 
using  namespace  va; 

int  main(  int  argc,  char*  argv[]  )  { 

//  specify  a  Rotation  from  an  axis  vector  and  rotation  angle 
Vector  u  =  normalize(  Vector(  1.,  1.,  1.  )  ); 
double  th  =  rad(  120.  ); 

if  (  argc  ==  5  )  { 

u  =  normalize(  Vector(  atof(  argv[l]  ),  atof(  argv[2]  ),  atof(  argv[3]  )  )  ); 
th  =  rad(  atof(  argv[4]  )  ); 

} 

std::cout  «  std : : setprecision (6)  «  std:: fixed  «  std : : showpos ; 

Rotation  R(  u,  th  ); 
matrix  A; 
quaternion  q; 

std::cout  «  "Given  the  Rotation:"  «  std::endl; 
std::cout  «  R  «  std::endl  «  std::endl; 

//  convert  Rotation  to  a  rotation  matrix 

std::cout  «  "convert  Rotation  to  rotation  matrix:"  «  std::endl; 

A  =  to_matrix(  R  ); 

std::cout  «  A  «  std::endl  «  std::endl; 

//  convert  a  Rotation  to  a  quaternion 

std::cout  «  "convert  Rotation  to  quaternion:"  «  std::endl; 

q  =  to_quaternion (  R  ); 

std::cout  «  q  «  std::endl  «  std::endl; 

std::cout  «  "Given  the  rotation  matrix:"  «  std::endl; 
std::cout  «  A  «  std::endl  «  std::endl; 

//convert  a  rotation  matrix  to  a  Rotation 
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std::cout  «  "convert  rotation  matrix  to  Rotation:"  «  std::endl; 

R  =  Rotation!  A  ) ; 

std::cout  «  R  «  std::endl  «  std::endl; 

//convert  a  rotation  matrix  to  a  quaternion 

std::cout  «  "convert  rotation  matrix  to  quaternion:"  «  std::endl; 
q  =  to_quaternion (  Rotation!  A  )  ); 
std::cout  «  q  «  std::endl  «  std::endl;; 

std::cout  «  "Given  the  quaternion:"  «  std::endl; 
std::cout  «  q  «  std::endl  «  std::endl; 

//  convert  a  quaternion  to  a  Rotation 

std::cout  «  "convert  quaternion  to  Rotation:"  «  std::endl; 

R  =  Rotation!  q  ) ; 

std::cout  «  R  «  std::endl  «  std::endl; 

//  convert  a  quaternion  to  a  rotation  matrix 

std::cout  «  "convert  quaternion  to  rotation  matrix:"  «  std::endl; 
A  =  to_matrix(  Rotation!  q  )  ) ; 
std::cout  «  A  «  std::endl; 

return  EXIT_ SUCCESS; 

} 


Compiling  this  program  with 

g++  -02  -Wall  -o  convert  convert. cpp  -Im 


and  then  running  it  via  the  command 

./convert  2.35  6.17  -4.6  35.6 


produces  the  following  output: 


Given  the  Rotation: 

+0.292041  +0.766762  -0.571654  +35.600000 

convert  Rotation  to  rotation  matrix: 

+0.829041  +0.374624  +0.415148 

-0.290921  +0.922983  -0.251926 

-0.477552  +0.088081  +0.874177 

convert  Rotation  to  quaternion: 

+0.952129  +0.089275  +0.234396  -0.174752 

Given  the  rotation  matrix: 

+0.829041  +0.374624  +0.415148 

-0.290921  +0.922983  -0.251926 

-0.477552  +0.088081  +0.874177 

convert  rotation  matrix  to  Rotation: 

+0.292041  +0.766762  -0.571654  +35.600000 

convert  rotation  matrix  to  quaternion: 
+0.952129  +0.089275  +0.234396  -0.174752 

Given  the  quaternion: 

+0.952129  +0.089275  +0.234396  -0.174752 

convert  quaternion  to  Rotation: 

+0.292041  +0.766762  -0.571654  +35.600000 

convert  quaternion  to  rotation  matrix: 
+0.829041  +0.374624  +0.415148 

-0.290921  +0.922983  -0.251926 

-0.477552  +0.088081  +0.874177 
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Appendix  G.  Slerp  (Spherical  Linear  Interpolation) 
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This  is  a  derivation  of  the  spherical  linear  interpolation  (Slerp)  formula  that  was 
introduced  by  Shomake.1  As  depicted  in  Fig.  G-l,  the  rotation  that  takes  the  unit 
vector  iii  to  the  unit  vector  u2  is  the  unit  quaternion 

0  9 

q  =  cos  -  +  nsin (G-l) 
where  6  is  the  angle  between  ii\  and  u2,  and 


U\  x  u2 

n  =  — - — 

Ui  X  u2 


(G-2) 


is  the  unit  vector  along  the  axis  of  rotation.  This  means  that 


u2  =  quiq 


(G-3) 


Now  let  us  parametrize  the  angle  as  t6,  where  0  <  £  <  1,  and  let 


t6  t9 

q(t)  =  cos  —  +  n  sin  — . 


(G-4) 


Then  an  intermediate  unit  vector  u(t)  that  runs  along  the  arc  on  the  unit  circle  from 
iii  to  ii2  is  given  by 


^  I  to  „  .  to\  „  (  to  ^  .  to\ 

u(t)  =  q[t)Uiq(t)  =  (  cos  —  +  n  sin  —  I  Ui  I  cos  — —  n  sin  —  ]  .  (G-5) 


'Shoemake  K.  Animating  rotation  with  quaternion  curves.  SIGGRAPH  ’85;  1985;245-254. 
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Treating  ii\  as  the  pure  quaternion  (0.  ii\ )  and  using  the  fact  that  Uj  h  —  0, 
U‘2  ■  h  =  0,  fi  ■  (n  x  iii)  =  0,  and  ||mi  x  m2||  =  sin#,  we  can  carry  out  the 
quaternion  multiplication  to  get 


to 


to 


u(t)  =  cos - fn  sin  —  ii\  I  cos - h  sin  — 


tO 


to 


to  ^  „  to 

=  I  cos  — tti  +  n  x  iti  sm  — 
'  2  2 


to 


to  .  to 


to  „  to 

cos - n  sm  — 

2  2 

to  .  to 


to 


=  cos  — Mi  +  cos  —  sm  — n  x  U\  +  cos  —  sm  — n  x  Mi  —  sm  —  n  x  mm  x  n 

2  22  22  2  v  ' 


9  tO  „  tO  tO  ^  „  9  to .  .  „  „  . 

=  cos  — Mi  +  2  cos  —  sm  — n  x  Mi  +  sin  — n  x  (n  x  u i ) . 
2  2  2  2  y  ’ 


(G-6) 


Now 


n  x  M!  =  —Ui  x  n  = 


iii  x  («i  x  m2) 

sin  0 

Ui(ui  ■  U2)  -  w2(ui  •  Mi) 


sin  0 


ii2  —  cos  0  iii 
sin  0 


(G-7) 


and 


n  x  (fi.  x  Mi)  =  n(n  ■  Mi)  —  M^n  ■  fi)  =  —Mi 


(G-8) 


so  that 


,  2  tO  2  £#\  „  tO  tO  ( m2  —  cos  0  iii 

“(*)=( cos  Y_sm  yJ“1+2cosysm2 1 — ss? — 


=  cos  tO  iii  +  sinf# 


n2  —  cos  0  Mi 
sin  0 


cos  tO  sin  0  Mi  +  sin  tO  ii2  —  sin  tO  cos  0  Mi 
sin  0 

sin(#  —  tO )  Mi  +  sin  tO  ii2 
sin  0 

sin(l  —  t)0  sin  tO 

— —q — Ul  +  •  ' 

sm  0  sm  0 


(G-9) 
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G-1.  Slerp  Formula 


Thus,  the  spherical  linear  interpolation  of  the  unit  vector  on  the  arc  of  the  unit  sphere 
from  iii t0  w2  is  given  by 


„  .  .  sin(l  —  t)9  ^  sin  t9  ^ 
u(t)  =  - — - u i  +  .  -  u2 


sin  9 


sin  9 


(G-10) 


where  0  <  t  <  1.  This  gives  us  the  C++  implementation  in  Listing  G-1. 


Listing  G-1.  slerp.cpp 


//  slerp.cpp:  original  slerp  formula 

#include  "Vector. h" 

#include  <iostream> 

#include  <cstdlib> 
using  namespace  va; 

int  main(  int  argc,  char*  argv[]  )  { 

Vector  i(  1.,  0.,  0.  ),  j(  0.,  1.,  0.  ),  k(  0.,  0.,  1.  ); 

Vector  ul  =  i;  //  default  initial  vector 

Vector  u2  =  j ;  //  default  final  vector 

if  (  argc  >  1  )  {  //or  specify  initial  and  final  vectors  on  the  command  line 

ul  =  Vector(  atof(  argv[l]  ),  atof(  argv[2]  ),  atof(  argv[3]  )  ); 
u2  =  Vector(  atof(  argv[4]  ),  atof(  argv[5]  ),  atof(  argv[6]  )  ); 

} 

const  int  N  =  1000; 

const  double  THETA  =  acos(  ul  *  u2  ) ; 

const  double  A  =  1.  /  sin(  THETA  ); 

Vector  u; 

double  t; 

for  (  int  n  =  0;  n  <  N;  n++  )  { 
t  =  doublet  n  )  /  doublet  N-l  ); 

u  =  A  *  (  sin(  (  1.  -  t  )  *  THETA  )  *  ul  +  sin(  t  *  THETA  )  *  u2  )  ; 
std::cout  «  u  «  std::endl; 

> 

return  EXIT-SUCCESS; 

} 


G-2.  Fast  Incremental  Slerp 

The  straightforward  application  of  Eq.  G-10,  as  we  incrementally  vary  t  from  0  to  1, 
involves  the  computationally  expensive  evaluation  of  trigonometric  functions  in  an 
inner  loop.  We  show  here  how  this  can  be  avoided.1 

Starting  with  Eq.  G-10,  using  the  double  angle  formula,  and 

-ui  ■  w2  =  cos6»,  (G-1 1) 


'Barrera  T,  Hast  A,  Bengtsson  E.  Incremental  spherical  linear  interpolation.  SIGRAD  2004. 
2005:7-10. 
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we  have1 


uit)  = 


sin  9  cos {tO)  —  cos  6  sin(t#)]  iii  +  sin  t6  ii2 
sin  6 


,  „  sin(t0)  . 

=  COS  (tv)  Ui  H - : — —  [U2  —  COS0«iJ 


sin  9 

=  cos (t9)  u\  +  sin(t9) 
=  cos (t9)  ii\  +  sin(t$) 


ii2  —  cos  Qiii 
L  y/1  —  cos2  9 
ii2  -  {iii  ■  u2)ui 
y/1  ~  (ui  ■  ii2)2 


(G-12) 


Now  consider  the  term  in  square  brackets.  The  numerator  is  ii2  minus  the  projection 
of  ii2  onto  iii,  and  thus  is  orthogonal  to  ii\.  Also,  the  denominator  is  the  norm  of 
the  numerator,  since 


[ii2  -  {iii  ■  u2)ui\  ■  [ii-2  ~  {iii  ■  u2)ui] 


l -{iii-  u2j2  -  {iii  ■  u2)2  +  {iii  ■  u2)2 
l -{iii-  u2)2-  (G-13) 


Thus,  the  term  in  square  brackets  is  a  fixed  unit  vector  that  is  tangent  to  ii\,  which 
we  label  ii0: 


Wo 


n  -  {Hi  •  ii2)ui 

\/l  -  {iii  ■  ii2)2 


(G-14) 


Therefore,  Eq.  G-12  can  be  written  as 


ii{t )  =  cos {t9)  iii  +  sin(f^)  ii0. 


(G-15) 


We  want  to  evaluate  ii  incrementally,  so  let  us  discretize  this  equation  by  setting 
59  =  9 /{N  —  1)  and  let  x  =  59.  Then  Eq.  G-15  becomes 


u[n]  =  cos {nx)  iii  +  sin {nx)  iio 


(G-16) 


for  n  =  0, 1,  2, . . . ,  N  —  1  . 


'Hast  A,  Barrera  T,  Bengtsson  E.  Shading  by  spherical  linear  interpolation  using  De  Moivre’s 
formula.  WSCG’03.  2003;Short  Paper;57-60. 

Approved  for  public  release;  distribution  is  unlimited. 


77 


1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

14 

15 

16 

17 

18 

19 

20 

21 

22 

23 

24 

25 


Now  we  make  use  of  the  trigonometric  identities 


cos(n  +  l)x  +  cos(n  —  l)x  =  2cosnxcosx, 
sin(n  +  l)x  +  sin(n  —  l)x  =  2  sin  nx  cos  x. 

Or,  changing  n  — *  n  —  1  and  rearranging, 

cos  nx  =  2  cos  x  cos (n  —  l)x  —  cos(n  —  2)x, 
sin  nx  =  2cosxsin(n  —  l)x  —  sin(n  —  2)x. 

Substituting  these  into  Eq.  G-16  results  in  a  simple  recurrence  relation: 

ii[n\  =  [2cosxcos(n  —  l)x  —  cos(n  —  2)x]  iii+ 

[2  cos  x  sin(n  —  l)x  —  sin(n  —  2)x]  ii0 
=  2  cosx[cos(n  —  l)x  iii  +  sin(n  —  l)x  tto]- 
[cos(n  —  2)x  iii  +  sin  (n  —  2)x  iio] 

=  2  cos  x  u  [n  —  1]  —  ii  [n  —  2] . 

It  is  also  easy  to  evaluate  the  first  2  values  directly  from  Eq.  G-16: 

u[0]  =  iii  and 

fi[l]  =  cosx  iii  +  sinx  fio- 

Putting  this  all  together  gives  us  the  C++  implementation  in  Listing  G-2. 

Listing  G-2.  fast_slerp.cpp 

//  fast_slerp.cpp:  fast  incremental  slerp  evaluates  trig  functions  only  once 
//  R.  Saucier,  June  2016 

#include  "Vector. h" 

#include  <iostream> 

#include  <cstdlib> 
using  namespace  va; 

int  main(  int  arge,  char*  argv[]  )  { 

Vector  i(  1.,  0.,  0.  ),  j(  0.,  1.,  0.  ),  k(  0.,  0.,  1.  ); 

Vector  ul  =  i;  //  default  initial  vector 
Vector  u2  =  j ;  //  default  final  vector 

if  (  arge  >  1  )  {  //or  specify  initial  and  final  vectors  on  the  command  line 

ul  =  Vector(  atof(  argv[l]  ),  atof(  argv[2]  ),  atof(  argv[3]  )  ); 
u2  =  Vector(  atof(  argv[4]  ),  atof(  argv[5]  ),  atof(  argv[6]  )  ); 

> 

const  int  N  =  1000; 

const  double  U12  =  ul  *  u2; 

const  double  THETA  =  acos(  U12  ); 

const  double  TH  =  THETA  /  double (  N-l  ); 

const  double  C  =  2.  *  cos(  TH  ); 
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const  Vector  U0 


=  (  u2  -  U12  *  ul  )  /  sqrt (  (  1.  -  U12  )  *  (  1.  +  U12  )  ) ; 


Vector  u_2,  u_l,  u; 

for  (  int  n  =  0;  n  <  N;  n++  )  { 

if  (  n  ==  0  )  u  =  u_2  =  ul;  //  n  =  0  will  become  u  at  n  -  2; 

else  if  (  n  ==  1  )  u  =  u_l  =  cos(  TH  )  *  ul  +  sin(  TH  )  *  U0;  //  n  =  1  will  become  u  at  n  -  1 
else  { 

u  =  C  *  u_l  -  u_2; 
u_2  =  u_l; 
u_l  =  u; 

} 

std::cout  «  u  «  std::endl; 


return  EXIT_SUCCESS; 

} 


Removing  the  trigonometric  functions  from  the  inner  loop  results  in  a  speedup  of 
about  12  times  over  the  original  slerp  formula  in  Eqs.  G- 10  or  G- 16. 
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Appendix  H.  Exact  Solution  to  the  Absolute  Orientation  Problem 
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This  solution  follows  the  approach  of  Micheals  and  Boult.1  Given  2  sets  of  3  linearly  inde¬ 
pendent  vectors,  {ai,  a2,  a3}  and  {&i,  b2,  b3},  where  the  2  sets  of  vectors  are  not  necessarily 
unit  vectors  but  are  related  by  a  pure  rotation,  the  absolute  orientation  problem  is  to  find 
this  rotation.  Since  rotations  can  be  represented  by  unit  quaternions,  there  must  be  a  unit 
quaternion  q  such  that 

bi  =  qa^1  for  i  =  1,  2,  3  (H-l) 

where  q  =  q0  +  gA  +  g2j  +  q3 k  and  q%  +  qj  +  q%  +  q3  =  1-  Using  12  =  j2  =  k2  =  ijk  =  — 1, 
q-1  =  q0  —  q\i  —  q2 j  —  g3k,  and  expanding,  we  get 


bx  =  axql  +  2 azq0q2  -  2ayq0q3  +  axqf  +  2ayq3q2  +  2azqxq3  -  axql  -  axql  (H-2) 

by  =  ayql  -  2azq0q1  +  2axq0q3  -  ayq\  +  2axq]  q2  +  ayq\  +  2 azq2q3  -  ayql  (H-3) 

bz  =  azql  +  2ayq0q1  -  2axq0q2  -  azq\  +  2axq1q3  -  azq\  +  2 ayq2q3  +  azq23  (H-4) 


for  each  of  the  3  vectors.  Imposing  the  normalization  condition  then  gives  us  10  equations 
in  10  unknowns: 
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The  10  x  10  coefficient  matrix  can  be  inverted  with  MATHEMATICA.  And  we  find  that  the 
solution  for  the  10  products  of  the  4  quaternion  components  can  be  expressed  in  terms  of 
scalar  triple  products  as  follows: 


2  _  det(ai,  a2,  a3)  +  det(6i,  a2,  a3)  +  det(ai,  b2,  a3)  +  det(ai,  a2,  b3 ) 
0  4det(ai,a2,a3) 


(H-6) 


al 


al 


al 


det(ai,  a2,  a3)  +  det(Pnbi,  a2,  a3)  +  det(ai,  Pnb2,  a3)  +  det(ai,  a2,  Pub3) 

4det(ai,  a2,  a3) 

det(ai,  a2,  a3)  +  det(P22bi,  a2,  a3)  +  det(ai,  P22b-2 ,  a3)  +  det(ai,  a2,  P22b3) 

4det(ai,  a2,  a3) 

det(ai,  a2,  a3)  +  det(P33bi,  a2,  a3)  +  det(ai,  P33b-2,  a3)  +  det(ai,  a2,  P33b3) 

4det(ai,  a2,  a3) 

_  det(Pjjbi,  a2,  a3)  +  det(ai,  Pjjb %  a3)  +  det(ai,  a2,  Ppb3) 

^  4det(ai,a2,a3) 


(H-7) 

(H-8) 

(H-9) 

(H-10) 


1  Micheals  RJ,  Boult  TE.  Increasing  robustness  in  self-localization  and  pose  estimation,  [date  un¬ 
known;  accessed  2010  Jun],  http://www.vast.uccs.edu/  tboult/PAPERS/SPIE99-Increasing-robustness-in- 

self-localization-and-pose-estimation-Micheals-Boult.pdf. 
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(H-ll) 


(H-12) 


(H-13) 


(H-14) 


We  set  q0  as  the  positive  square  root  of  Eq.  H-6  and  then  use  Eq.  H-10  to  get  qi  =  qoqi/qo, 
q2  =  qoq-2 /do ,  and  q3  =  qoq3/qo-  The  axis  of  rotation  is  along  the  unit  vector 


gii  +  ddj  +  93k 


and  the  angle  of  rotation  is 


The  full  rotation  matrix  is 


6  =  2  cos  1  q0- 


R  = 


'2 q20  -  1  +  2 qi 

2qiq2  +  2q$q3 
2qlqz  -  2q0q2 


2q1q2  -  2 q0q3 
2  ql  -  1  +  2  q22 
2q2q3  +  2q0q1 


2did3  +  2q,oQ,2 
2q2q3  ~  2<Mi 
2  ql  -  1  +  2  ql 


(H-15) 


(H-16) 


(H-17) 
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The  program  in  Listing  H-l  is  designed  to  test  the  implemented  closed- form  solution. 


Listing  H-l.  ao.cpp 

//  ao.cpp:  test  of  absolute  orientation  as  implemented  in  Rotation  class 

#include  "Rotation. h" 

#include  <iostream> 

#include  <cstdlib> 

#include  <iomanip> 

using  namespace  va;  //  vector  algebra  namespace 
int  main(  int  argc,  char*  argv[]  )  { 

Vector  al(  3.5,  1.0,  2.3  ),  a2(  1.5,  2.1,  7.1  ),  a3(  4.3,  -5.8,  1.7  );  //  3  linearly  independent  vectors 

Vector  bl,  b2,  b3; 

double  pitch  =  rad(  45.  );  //  default  pitch  (deg  converted  to  radians) 

double  yaw  =  rad(  -30.  );  //  default  yaw  (deg  converted  to  radians) 

double  roll  =  rad(  60.  );  //  default  roll  (deg  converted  to  radians) 

if  (  argc  ==  4  )  {  //or  specify  pitch,  yaw,  roll  (deg)  on  command  line 

pitch  =  rad(  atof(  argv[l]  )  ); 

yaw  =  rad(  atof(  argv[2]  )  ); 

roll  =  rad(  atof(  argv[3]  )  ); 

> 

std::cout  «  std : : setprecision (6)  «  std:: fixed  «  std : : showpos ; 

std::cout  «  "The  3  vectors  are  linearly  independent  iff  det(al,a2,a3)  is  non-zero: 

std::cout  «  ndet(al,a2,a3)  =  "  «  (  al  *  (  a2  ~  a3  )  )  «  std::endl  «  std::endl; 

//  perform  rotation  sequence  on  initial  vectors 
Rotation  Rl(  pitch,  yaw,  roll,  XYZ  ); 
bl  =  R1  *  al; 
b2  =  R1  *  a2; 
b3  =  R1  *  a3; 

//  output  the  rotated  vectors 

std::cout  «  "The  rotated  vectors  are:"  «  std::endl; 
std::cout  «  "bl  =  "  «  bl  «  std::endl; 
std::cout  «  "b2  =  "  «  b2  «  std::endl; 
std::cout  «  "b3  =  "  «  b3  «  std::endl  «  std::endl; 

//  given  only  the  two  sets  of  vectors,  find  the  rotation  that  takes  {al,a2,a3}  to  {bl,b2,b3} 

Rotation  R2(  al,  a2,  a3,  bl,  b2,  b3  ); 

//  apply  this  rotation  to  the  original  vectors 
bl  =  R2  *  al; 
b2  =  R2  *  a2; 
b3  =  R2  *  a3; 

//  output  rotated  vectors  to  show  they  match  previous  output 

std::cout  «  "The  following  vectors  should  match  those  above:"  «  std::endl; 

std::cout  «  "bl  =  "  «  bl  «  std::endl; 

std::cout  «  "b2  =  "  «  b2  «  std::endl; 

std::cout  «  "b3  =  "  «  b3  «  std::endl  «  std::endl; 

//  factor  this  rotation  into  a  pitch -yaw- roll  rotation  sequence 
sequence  s  =  factor(  R2,  XYZ  ); 

//  output  rotation  sequence  to  show  it  matches  the  input  values 

std::cout  «  "Factoring  this  rotation  into  a  rotation  sequence  gives:"  «  std::endl; 

std::cout  «  "pitch  =  "  «  deg(  s. first  )  «  std::endl; 

std::cout  «  "yaw  =  "  «  deg(  s. second  )  «  std::endl; 

std::cout  «  "roll  =  "  «  deg(  s. third  )  «  std::endl; 

return  EXIT-SUCCESS; 

} 


Compiling  this  program  with 

g++  -02  -Wall  -o  ao  ao.cpp  -Im 


and  then  running  it  with  the  command 

./ao  -35.2  43.5  -75.6 
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prints  out  the  following: 


The  3  vectors  are  linearly  independent  iff  det(al,a2,a3)  is  non-zero:  det(al,a2,a3)  =  +143.826000 

The  rotated  vectors  are: 
bl  =  +2.917177  -2.334937  +2.139660 
b2  =  +6.633337  +1.253165  +3.390932 
b3  =  -2.129101  -2.066399  +6.798303 

The  following  vectors  should  match  those  above: 
bl  =  +2.917177  -2.334937  +2.139660 
b2  =  +6.633337  +1.253165  +3.390932 
b3  =  -2.129101  -2.066399  +6.798303 

Factoring  this  rotation  into  a  rotation  sequence  gives: 
pitch  =  -35.200000 
yaw  =  +43.500000 
roll  =  -75.600000 


The  3  vectors  need  not  be  orthonormal — nor  even  mutually  orthogonal — but  they 
must  be  linearly  independent.  A  necessary  and  sufficient  condition  for  this  is 
det(ai,  a2,  a3)  ^  0.  We  see  that  is  the  case  from  line  1.  Lines  4-6  show  the 
effect  of  the  given  rotation  sequence  upon  the  original  3  vectors  (see  lines  28-38 
of  Listing  H-l).  The  program  then  computes  the  rotation  that  will  take  the  original 
vectors  to  these  3  vectors  (line  41  of  Listing  H-l)  and  then  applies  it  to  the  original 
vectors  (lines  43-52  of  Listing  H-l).  We  see  on  lines  9-1 1  that  these  do  indeed  match 
lines  4-6.  Finally,  the  program  factors  the  computed  rotation  into  a  pitch-yaw-roll 
rotation  sequence  (line  55  of  Listing  H-l)  and  the  lines  14-16  show  that  we  retrieve 
the  input  values.  Thus  we  verify  that  the  program  is  able  to  find  the  rotation  as  long 
as  the  original  vectors  are  linearly  independent. 
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